JP2017228318A - Control device for link operation device - Google Patents
Control device for link operation device Download PDFInfo
- Publication number
- JP2017228318A JP2017228318A JP2017180835A JP2017180835A JP2017228318A JP 2017228318 A JP2017228318 A JP 2017228318A JP 2017180835 A JP2017180835 A JP 2017180835A JP 2017180835 A JP2017180835 A JP 2017180835A JP 2017228318 A JP2017228318 A JP 2017228318A
- Authority
- JP
- Japan
- Prior art keywords
- link
- time
- control
- deceleration
- acceleration
- 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
- 230000001133 acceleration Effects 0.000 claims abstract description 138
- 230000007246 mechanism Effects 0.000 claims abstract description 48
- 230000008859 change Effects 0.000 claims abstract description 40
- 206010034719 Personality change Diseases 0.000 abstract description 9
- 230000036544 posture Effects 0.000 description 49
- 238000000034 method Methods 0.000 description 15
- 238000006243 chemical reaction Methods 0.000 description 11
- 230000009467 reduction Effects 0.000 description 5
- 238000005452 bending Methods 0.000 description 4
- 239000012636 effector Substances 0.000 description 3
- 230000001360 synchronised effect Effects 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000036316 preload Effects 0.000 description 2
- 238000000926 separation method Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 230000008094 contradictory effect Effects 0.000 description 1
- 230000006866 deterioration Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000004904 shortening Methods 0.000 description 1
- 125000006850 spacer group Chemical group 0.000 description 1
Images
Landscapes
- Transmission Devices (AREA)
- Control Of Position Or Direction (AREA)
Abstract
Description
この発明は、産業機器等の精密で広範な作動範囲を必要とする機器に用いられるリンク作動装置の制御装置に関する。 The present invention relates to a control device for a link actuating device used for equipment that requires a precise and wide working range such as industrial equipment.
この種のリンク作動装置として、基端側のリンクハブに対し先端側のリンクハブを、３組以上のリンク機構を介して姿勢を変更可能に連結したものが提案されている（例えば特許文献１）。前記各リンク機構は、それぞれ前記基端側のリンクハブおよび先端側のリンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とでなる。各リンク機構は、このリンク機構を直線で表現した幾何学モデルが、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状である。前記３組以上のリンク機構のうち２組以上のリンク機構に、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を任意に変更させるアクチュエータが設けられる。 As this type of link actuating device, a device has been proposed in which a distal end side link hub is connected to a proximal end side link hub via three or more sets of link mechanisms so that the posture can be changed (for example, Patent Document 1). ). Each of the link mechanisms includes a base end side and a front end side end link member, one end of which is rotatably connected to the base end side link hub and the front end side link hub, and the base end side and the front end side of the link mechanism. It consists of a central link member having both ends rotatably connected to the other end of the end link member. Each link mechanism has a geometric model in which the link mechanism is expressed by a straight line in which the proximal end portion and the distal end portion are symmetrical with respect to the central portion of the central link member. Of the three or more sets of link mechanisms, two or more sets of link mechanisms are provided with actuators that arbitrarily change the attitude of the distal end side link hub with respect to the proximal end side link hub.
このリンク作動装置を制御する制御装置は、アクチュエータの全てが同時に動作を開始して同時に動作を完了するように制御する同期制御により、前記アクチュエータごとの動作を制御して、前記先端側のリンクハブを任意の姿勢に変更する姿勢制御を行う構成とされる。 The control device for controlling the link actuating device controls the operation of each actuator by synchronous control for controlling all of the actuators to start operation and complete the operation at the same time. It is set as the structure which performs attitude | position control which changes to arbitrary attitude | positions.
前記リンク作動装置の制御装置として、前記全てのアクチュエータの減速時間を、前記リンク作動装置の持つ共振周波数の１周期付近に設定して、前記同期制御および姿勢制御を行うことが試みられている。なお、ここで言う共振周波数は、先端側のリンクハブに搭載物を設置した状態での共振周波数のことである。 As a control device for the link actuator, an attempt is made to perform the synchronization control and the attitude control by setting the deceleration time of all the actuators to be close to one cycle of the resonance frequency of the link actuator. In addition, the resonance frequency said here is a resonance frequency in the state which installed the object in the link hub of the front end side.
前記構成のリンク作動装置においては、直線加減速制御で先端部を駆動する場合、特に先端部の負荷が大きい場合、リンク部の剛性により先端部の振動が収まる整定時間が長くなるという問題がある。その対策の一つとしてリンクハブの姿勢変更時のアクチュエータの加速時間と減速時間を、リンク作動装置のもつ共振周波数（リンク作動装置の先端側リンクハブに搭載物を設置した状態での共振周波数）の１周期付近に設定することで、先端部が共振周波数の約半周期後に加速を打ち消す方向に振動し、整定時間を短縮させる方法がある。
しかし、リンク姿勢変更時の移動量が小さく、指令速度が大きい条件では、リンクの動作時間が加速時間と減速時間の合計以下になり、リンク先端部となる先端側リンクハブが振動して整定時間が伸び、結果として動作時間が長くなる場合がある。
In the link operating device having the above-described configuration, when the tip is driven by linear acceleration / deceleration control, particularly when the load on the tip is large, there is a problem that the settling time in which the vibration of the tip is settled is long due to the rigidity of the link. . As one of the countermeasures, the acceleration time and deceleration time of the actuator when changing the attitude of the link hub, the resonance frequency of the link actuator (resonance frequency with the load installed on the link hub on the tip side of the link actuator) There is a method in which the settling time is shortened by setting the vicinity of one cycle of the tip portion so that the tip portion vibrates in a direction to cancel acceleration after about a half cycle of the resonance frequency.
However, under the condition that the movement amount when changing the link posture is small and the command speed is large, the link operation time becomes less than the sum of the acceleration time and deceleration time, and the tip side link hub that becomes the link tip part vibrates and settles. May increase, resulting in a longer operating time.
この発明の目的は、リンク姿勢変更時の移動量が小さく、指令速度が大きい条件において、自動で制御条件を変更して先端側リンクハブの整定時間を短くすることができるリンク作動装置の制御装置を提供することである。 An object of the present invention is to provide a control device for a link actuating device capable of shortening the settling time of the distal end side link hub by automatically changing the control condition under the condition that the movement amount at the time of changing the link posture is small and the command speed is large. Is to provide.
この発明のリンク作動装置の制御装置は、基端側のリンクハブ１４に対し先端側のリンクハブ１５を、３組以上のリンク機構１１〜１３を介して姿勢を変更可能に連結し、前記各リンク機構１１〜１３は、それぞれ前記基端側のリンクハブ１４および先端側のリンクハブ１５に一端が回転可能に連結された基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂと、これら基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂの他端に両端がそれぞれ回転可能に連結された中央リンク部材１１ｃ〜１３ｃとでなり、前記各リンク機構１１〜１３は、このリンク機構１１〜１３を直線で表現した幾何学モデルが、前記中央リンク部材１１ｃ〜１３ｃの中央部に対する基端側部分と先端側部分とが対称を成す形状であり、前記３組以上のリンク機構１１〜１３のそれぞれに、前記基端側の端部リンク部材であるアーム１１ａ〜１３ａを回転させることにより、前記基端側のリンクハブ１４に対する前記先端側のリンクハブ１５の姿勢を任意に変更させるアクチュエータ３を設けたリンク作動装置において、
前記基端側のリンクハブ１４に対する前記先端側のリンクハブ１５の姿勢を、現在姿勢から指令された目標姿勢へ変更させるように、前記各アクチュエータ３を制御するリンク作動装置の制御装置４であって、
次の基本構成を備える。
The control device for the link actuating device according to the present invention connects the distal end
It is a
The following basic configuration is provided.
この基本構成は、前記各アクチュエータ３を、前記現在姿勢であるときの前記アーム１１ａ〜１３ａの回転角から前記目標姿勢となるときの前記アーム１１ａ〜１３ａの回転角までの指令アーム回転角度だけ、アーム回転制御の制御パラメータである加速時間、減速時間、および指令速度に従って、互いに同期して駆動する姿勢変更制御手段４１を備え、
前記制御パラメータのうち、初期パラメータとして加速時間Ｔacc 、減速時間Ｔdccを設定し、
前記アクチュエータ３の加速時間Ｔacc 、減速時間Ｔdcc 、指令速度Ｖ０、および指令アーム回転角度から推定動作時間Ｔｅを推定する動作時間推定手段４９と、
この推定された推定動作時間Ｔｅと加速時間Ｔacc および減速時間Ｔdcc の合計とを比較する条件選定手段５０と、
この条件選定手段５０による比較の結果に応じて定められた規則に従い前記制御パラメータを変更するパラメータ変更手段５１とを設けた構成である。
In this basic configuration, each
Among the control parameters, acceleration time Tacc and deceleration time Tdcc are set as initial parameters,
An operation time estimating means 49 for estimating an estimated operation time Te from the acceleration time Tacc, the deceleration time Tdcc, the command speed V0, and the command arm rotation angle of the
A condition selecting means 50 for comparing the estimated operation time Te with the sum of the acceleration time Tacc and the deceleration time Tdcc;
A parameter changing
この構成の制御装置４によると、初期の制御パラメータのうち加速時間Ｔacc と減速時間Ｔdcc を共振周波数の１周期で設定し、かつ直線加減速制御で駆動させる制御装置４において、アクチュエータ３の加速時間Ｔacc 及び減速時間Ｔdcc 、指令速度Ｖ０、指令アーム回転角度から推定動作時間を推定する動作時間推定手段４９により推定動作時間を求める。この求めた推定動作時間Ｔｅと加速時間Ｔacc 及び減速時間Ｔdcc の合計とを比較する条件選定手段５０の結果をもとに制御パラメータを変更するパラメータ変更手段５１を用いて、その動作時の制御パラメータを変更し、姿勢変更制御手段４１で駆動する。これにより、先端側リンクハブ１５の整定時間が短くなり、この整定時間を含むリンク姿勢変更の動作時間を短縮することができる。
According to the
前記基本構成において、前記パラメータ変更手段５１は、前記条件選定手段５０による比較の結果、前記推定動作時間Ｔｅが前記加速時間Ｔacc および減速時間Ｔdcc の合計以下であって、かつ前記推定動作時間Ｔｅがある所定の時間範囲にある場合に前記制御パラメータの変更を行うようにしても良い。例えば、前記指令速度を低下させる。このように指令速度を低下させることにより、加速時間Ｔaccと減速時間Ｔdccの合計をリンク作動装置が持つ共振周波数の１周期付近に設定すれば整定時間が短くなり、通常制御ではリンク先端の振動が大きく整定時間が長くなる場合と比較して、この整定時間を含むリンク姿勢変更動作時間を短縮することができる。なお、駆動条件によっては加速時間と減速時間の合計が共振周波数の１周期でなくても振動が大きくならない場合がある（例えば、低速時や１周期近辺等。）。よって、パラメータ変更条件は所定の時間範囲内とすることが好ましい。
In the basic configuration, as a result of the comparison by the
前記「ある所定の時間」は、例えば、所定時間範囲内の時間で加速時間と減速時間を設定すると位置決め完了後のリンク先端の整定時間悪化に影響を及ぼすことになる時間とする。 The “certain predetermined time” is, for example, a time that will affect the deterioration of the settling time of the link tip after completion of positioning if the acceleration time and the deceleration time are set within a predetermined time range.
また、前記基本構成の制御装置において、前記推定動作時間が加速時間と減速時間との合計以上となる場合に、前記パラメータ変更手段５１は、制御パラメータのうちの指令速度Ｖ１を、初期設定した指令速度Ｖ０よりも速い速度に変更しても良い。
これにより、例えば、前記加速時間と減速時間を維持したままで、リンク姿勢変更の動作時間を短縮することができる。
Further, in the control device having the basic configuration, when the estimated operation time is equal to or greater than the sum of the acceleration time and the deceleration time, the
Thereby, for example, the operation time for changing the link posture can be shortened while maintaining the acceleration time and the deceleration time.
この場合に、前記パラメータ変更手段は、前記推定動作時間が加速時間と減速時間との合計と等しくなるように、前記指令速度を速く設定しても良い。
この場合、前記加速時間Ｔacc と減速時間Ｔdcc を維持したままで、リンク姿勢変更の動作時間が最短になるように短縮することができる。
In this case, the parameter changing means may set the command speed fast so that the estimated operation time becomes equal to the sum of the acceleration time and the deceleration time.
In this case, while maintaining the acceleration time Tacc and the deceleration time Tdcc, the link posture changing operation time can be shortened.
この発明の制御装置において、前記推定動作時間が加速時間と減速時間との合計よりも小さい場合、前記パラメータ変更手段５１は、前記制御パラメータのうちの前記指令速度を、前記アクチュエータ３の動作が所定の推定動作時間Ｔｅとなるような指令速度に低下させる。
この場合、リンク先端の振動を抑制でき、整定時間を含む動作時間を短縮できるという効果が得られる。
この場合に、前記所定の推定動作時間Ｔｅは、加速時間Ｔacc と減速時間Ｔdcc の合計としても良い。
一般に移動距離が短い場合、モータは加速した後、中間地点で減速に切替わる。動作時間は加速時間と減速時間の合計となり、この合計時間がリンク装置の共振周波数の１周期分から大きく乖離するとリンク先端は位置決め完了後に大きく振動する。しかし、振動を抑制する動作時間（加速時間と減速時間の合計）をリンク装置の共振周波数の１周期に合わせると振動は抑制できる。
In the control device of the present invention, when the estimated operation time is smaller than the sum of the acceleration time and the deceleration time, the parameter changing
In this case, it is possible to suppress the vibration at the link tip and to shorten the operation time including the settling time.
In this case, the predetermined estimated operation time Te may be the sum of the acceleration time Tacc and the deceleration time Tdcc.
In general, when the moving distance is short, the motor accelerates and then switches to deceleration at an intermediate point. The operation time is the sum of the acceleration time and the deceleration time. If this total time deviates greatly from one period of the resonance frequency of the link device, the link tip vibrates greatly after the positioning is completed. However, the vibration can be suppressed by adjusting the operation time for suppressing the vibration (the sum of the acceleration time and the deceleration time) to one cycle of the resonance frequency of the link device.
この発明の制御装置において、前記パラメータ変更手段５１は、前記指令速度を、この指令速度の初期速度と移動量である指令アーム回転角度から求めた推定動作時間に比例した割合で減速した速度に変更しても良い。 In the control device of the present invention, the parameter changing means 51 changes the command speed to a speed that is decelerated at a rate proportional to an estimated operation time obtained from an initial speed of the command speed and a command arm rotation angle that is a movement amount. You may do it.
この発明の制御装置において、前記パラメータ変更手段５１は、前記指令速度を、初期速度に対して所定の割合で減速した速度に変更するようにしても良い。
In the control device of the present invention, the
この発明の制御装置において、前記パラメータ変更手段５１は、制御パラメータの変更により、前記姿勢変更制御手段４１による前記直線加減速制御をＳ字加減速制御に変更するようにしても良い。
Ｓ字加減速制御であると、制御は複雑となるが、加減速を急加速度で行うようにしても、加減速の完了時における整定時間がより一層短縮される。
In the control device of the present invention, the parameter changing means 51 may change the linear acceleration / deceleration control by the attitude change control means 41 to S-shaped acceleration / deceleration control by changing a control parameter.
In the S-curve acceleration / deceleration control, the control is complicated, but even if acceleration / deceleration is performed at a rapid acceleration, the settling time at the completion of acceleration / deceleration is further shortened.
この発明の制御装置において、前記パラメータ変更手段５１は、制御パラメータの変更により、前記姿勢変更制御手段による前記直線加減速制御を指数関数型の加減速制御にするようにしても良い。
指数関数型の加減速制御にした場合も、加減速を急加速度で行うようにしても、加減速の完了時における整定時間がより一層短縮される。
In the control device of the present invention, the parameter changing means 51 may change the linear acceleration / deceleration control by the attitude change control means to exponential acceleration / deceleration control by changing a control parameter.
Even in the case of exponential acceleration / deceleration control, even if acceleration / deceleration is performed at a rapid acceleration, the settling time at the completion of acceleration / deceleration is further reduced.
この発明の制御装置において、前記パラメータ変更手段５１は、加減速制御の時定数を変える場合に、加減速制御の時定数を推定動作時間に比例した割合で設定した時定数としても良い。 In the control device according to the present invention, the parameter changing means 51 may set the time constant of acceleration / deceleration control as a time constant set at a rate proportional to the estimated operation time when changing the time constant of acceleration / deceleration control.
この発明の制御装置４において、前記パラメータ変更手段５１は、加速時間または減速時間が固定で設定されている場合、指令速度を変更する。
In the
この発明の制御装置４において、前記パラメータ変更手段５１は、前記指令速度の代わりに加減速度を変更して各条件を満たす制御を行うようにしても良い。
なお、この発明で必要なことは加速時間と減速時間の合計が共振周波数の１周期となるように制御して、位置決め完了後のリンク先端の振動を抑制することである。そのための速度パラメータは加速・減速時間と（到達）速度、加速度・減速度と（到達）速度のいずれであっても問題がない。
In the
In the present invention, it is necessary to control the sum of the acceleration time and the deceleration time so as to be one period of the resonance frequency to suppress the vibration of the link tip after the positioning is completed. There are no problems whether the speed parameter for that purpose is acceleration / deceleration time and (arrival) speed, or acceleration / deceleration and (arrival) speed.
この発明のリンク作動装置の制御装置は、基端側のリンクハブに対し先端側のリンクハブを、３組以上のリンク機構を介して姿勢を変更可能に連結し、前記各リンク機構は、それぞれ前記基端側のリンクハブおよび先端側のリンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とでなり、前記各リンク機構は、このリンク機構を直線で表現した幾何学モデルが、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であり、前記３組以上のリンク機構のそれぞれに、前記基端側の端部リンク部材であるアームを回転させることにより、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を任意に変更させるアクチュエータを設けたリンク作動装置において、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を、現在姿勢から指令された目標姿勢へ変更させるように、前記各アクチュエータを制御するリンク作動装置の制御装置であって、前記各アクチュエータを、前記現在姿勢であるときの前記アームの回転角から前記目標姿勢となるときの前記アームの回転角までの指令アーム回転角度だけ、アーム回転制御の制御パラメータである加速時間、減速時間、および指令速度に従って、互いに同期して駆動する姿勢変更制御手段を備え、前記制御パラメータのうち、初期パラメータとして加速時間、減速時間を設定し、前記アクチュエータの加速時間、減速時間、指令速度、および指令アーム回転角度から動作時間を推定する動作時間推定手段と、この推定された推定動作時間と加速時間および減速時間の合計とを比較する条件選定手段と、この条件選定手段による比較の結果に応じて定められた規則に従い前記制御パラメータを変更するパラメータ変更手段とを設けたため、リンク姿勢変更時の移動量が小さく、設定速度が速い条件において、自動で制御条件を変更して先端側リンクハブの整定時間を短くすることができる。 The control device for the link actuating device according to the present invention connects the link hub on the distal end side to the link hub on the proximal end side so that the posture can be changed via three or more sets of link mechanisms, The proximal end and distal end end link members, one end of which is rotatably connected to the proximal end link hub and the distal end link hub, and the other ends of the proximal end and distal end end link members And each link mechanism has a geometric model expressing the link mechanism as a straight line, a proximal end portion and a distal end with respect to a central portion of the central link member. The side portion has a symmetrical shape, and each of the three or more sets of link mechanisms rotates an arm that is an end link member on the base end side to thereby rotate the tip with respect to the link hub on the base end side. In a link operating device provided with an actuator for arbitrarily changing the attitude of the link hub on the side, the attitude of the link hub on the distal end side with respect to the link hub on the proximal end side is changed from the current attitude to the commanded target attitude. A control device for a link actuating device for controlling each actuator, wherein each actuator is moved from a rotation angle of the arm when the current posture is set to a rotation angle of the arm when the target posture is set. At the command arm rotation angle, it is provided with posture change control means for driving in synchronization with each other in accordance with the acceleration time, deceleration time, and command speed, which are control parameters for arm rotation control, among the control parameters, acceleration time as an initial parameter, Set the deceleration time, and the acceleration time, deceleration time, command speed, and command Depending on the result of the comparison by the condition selection means, the operation time estimation means for estimating the operation time from the rotation angle, the condition selection means for comparing the estimated estimated operation time with the sum of the acceleration time and the deceleration time. Since the parameter changing means for changing the control parameter according to a predetermined rule is provided, the control condition is automatically changed under the condition that the moving amount at the time of changing the link posture is small and the setting speed is high, and the leading end side link hub is set. Time can be shortened.
この発明の実施形態を図面と共に説明する。まず、制御対象となるリンク作動装置を、図１ないし図５と共に説明する。図１および図２に示すように、このリンク作動装置１は、リンク作動装置本体２と、このリンク作動装置本体２を作動させる複数の姿勢制御用のアクチュエータ３とを備え、これら姿勢制御用のアクチュエータ３が制御装置４により制御される。図の例では、リンク作動装置本体２が、その基端側で、支持部材５にスペーサ６を介して吊り下げ状態に設置されている。リンク作動装置本体２の先端側には、先端取付部材７を介してエンドエフェクタ８が搭載されている。
An embodiment of the present invention will be described with reference to the drawings. First, the link actuating device to be controlled will be described with reference to FIGS. As shown in FIGS. 1 and 2, the link operating device 1 includes a link operating device
リンク作動装置本体２は、３組のリンク機構１１，１２，１３（以下、「１１〜１３」と表記する）を具備する。なお、図１では、リンク機構１１の形状につき１組のみを表示し、図１では他の２組のリンク機構１２，１３について、制御の説明のためにブロックで示している。これら３組のリンク機構１１〜１３のそれぞれは幾何学的に互いに同一形状をなす。すなわち、各リンク機構１１〜１３は、後述のように各リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂ，１１ｃ〜１３ｃを直線で表現した幾何学モデルが、中央リンク部材１１ｃ〜１３ｃの中央部に対する基端側部分と先端側部分が対称を成す形状である。
The link actuator
各リンク機構１１，１２，１３は、基端側の端部リンク部材１１ａ，１２ａ，１３ａ（以下、「１１ａ〜１３ａ」と表記する）、中央リンク部材１１ｃ，１２ｃ，１３ｃ（以下、「１１ｃ〜１３ｃ」と表記する）、および先端側の端部リンク部材１１ｂ，１２ｂ，１３ｂ（以下、「１１ｂ〜１３ｂ」と表記する）で構成され、４つの回転対偶からなる３節連鎖のリンク機構をなす。基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂはＬ字状をなし、基端がそれぞれ基端側のリンクハブ１４および先端側のリンクハブ１５に回転自在に連結されている。中央リンク部材１１ｃ〜１３ｃは、両端に基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂの先端がそれぞれ回転自在に連結されている。なお、端部リンク部材１１ａ〜１３ａは、請求項で言う「アーム」であり、以下の説明において、「アーム１１ａ〜１３ａ」と称する場合がある。
Each of the
基端側のリンクハブ１４および先端側のリンクハブ１５は六角柱状で、外周面を構成する６つの側面１６のうちの１つ置きに離れた３つの側面１６に、基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂがそれぞれ回転自在に連結されている。３組のリンク機構１１〜１３の各基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂは、球面リンク機構と呼ばれる構造であって、それぞれの球面リンク中心ＰＡ，ＰＢ（図１）は一致しており、また、その球面リンク中心ＰＡ，ＰＢからの距離も同じである。端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂと中央リンク部材１１ｃ〜１３ｃとの連結部となる回転対偶軸は、ある交差角をもっていてもよいし、平行であってもよい。
The
つまり、３組のリンク機構１１〜１３は、幾何学的に同一形状をなす。幾何学的に同一形状とは、各リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂ，１１ｃ〜１３ｃを直線で表現した幾何学モデル、すなわち各回転対偶と、これら回転対偶間を結ぶ直線とで表現したモデルが、中央リンク部材１１ｃ〜１３ｃの中央部に対する基端側部分と先端側部分が対称を成す形状であることを言う。図４は、一つのリンク機構１１を直線で表現した図である。
That is, the three sets of
この実施形態のリンク機構１１〜１３は回転対称タイプで、基端側のリンクハブ１４および基端側の端部リンク部材１１ａ〜１３ａと、先端側のリンクハブ１５および先端側の端部リンク部材１１ｂ〜１３ｂとの位置関係が、中央リンク部材１１ｃ〜１３ｃの中心線Ｃ（図４）に対して回転対称となる位置構成になっている。図３に示すように、各リンク機構１１〜１３の姿勢が変化しても、基端側と先端側の球面リンク中心ＰＡ，ＰＢ間の距離Ｈは変化しない。
The
図５は、基端側のリンクハブ１４と基端側の端部リンク部材１１ａ〜１３ａの連結部を示す断面図である。基端側のリンクハブ１４の側面１６から軸部１８が突出し、この軸部１８に複列の軸受１７の内輪（図示せず）が外嵌し、基端側の端部リンク部材１１ａ〜１３ａの基端側のリンクハブ側の端部に軸受１７の外輪（図示せず）が内嵌している。つまり、内輪は基端側のリンクハブ１４に固定され、外輪が基端側の端部リンク部材１１ａ〜１３ａと共に回転する構造である。軸受１７は、例えば深溝玉軸受、アンギュラ玉軸受等の玉軸受であって、ナット１９による締付けでもって所定の予圧量を付与して固定されている。軸受１７としては、図示例のように玉軸受を複列で配列する以外に、ローラ軸受や滑り軸受を用いてもよい。先端側のリンクハブ１５と先端側の端部リンク部材１１ｂ〜１３ｂの連結部も、同様の構造である。
FIG. 5 is a cross-sectional view showing a connecting portion between the proximal-
また、基端側の端部リンク部材１１ａ〜１３ａと中央リンク部材１１ｃ〜１３ｃの連結部も複列の軸受２０を介して互いに回転自在に連結されている。すなわち、基端側の端部リンク部材１１ａ〜１３ａに軸受２０の外輪（図示せず）が外嵌し、中央リンク部材１１ｃ〜１３ｃに設けた軸部２１に軸受２０の内輪（図示せず）が外嵌している。軸受２０は、例えば深溝玉軸受、アンギュラ玉軸受等の玉軸受であって、ナット２２による締付けでもって所定の予圧量を付与して固定されている。軸受２０としては、図示例のように玉軸受を複列で配列する以外に、ローラ軸受や滑り軸受を用いてもよい。先端側の端部リンク部材１１ｂ〜１３ｂと中央リンク部材１１ｃ〜１３ｃの連結部も、同様の構造である。
Further, the connecting portions of the base end side
前記リンク機構１１〜１３において、端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂの軸部１８の角度、長さ、および端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂの幾何学的形状が基端側と先端側で等しく、また、中央リンク部材１１ｃ〜１３ｃについても基端側と先端側で形状が等しいとき、中央リンク部材１１ｃ〜１３ｃの対称面に対して中央リンク部材１１ｃ〜１３ｃと、基端側および先端側のリンクハブ１４，１５と連結される端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂとの角度位置関係を基端側と先端側で同じにすれば、幾何学的対称性から基端側のリンクハブ１４および基端側の端部リンク部材１１ａ〜１３ａと、先端側のリンクハブ１５および先端側の端部リンク部材１１ｂ〜１３ｂとは同じに動き、基端側と先端側は同じ回転角になって等速で回転することになる。この等速回転するときの中央リンク部材１１ｃ〜１３ｃの対称面を等速二等分面という。
In the
このため、基端側のリンクハブ１４および先端側のリンクハブ１５を共有する同じ幾何学形状のリンク機構１１〜１３を円周上に複数配置させることにより、複数のリンク機構１１〜１３が矛盾なく動ける位置として中央リンク部材１１ｃ〜１３ｃが等速二等分面上のみの動きに限定され、これにより基端側と先端側は任意の作動角をとっても等速回転が得られる。
For this reason, the plurality of
各リンク機構１１〜１３における４つの回転対偶の回転部、つまり、基端側のリンクハブ１４と基端側の端部リンク部材１１ａ〜１３ａとの連結部分、先端側のリンクハブ１５と先端側の端部リンク部材１１ｂ〜１３ｂとの連結部分、および基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂと中央リンク部材１１ｃ〜１３ｃとの２つの連結部分を軸受構造とすることにより、その連結部分での摩擦抵抗を抑えて回転抵抗の軽減を図ることができ、滑らかな動力伝達を確保できると共に耐久性を向上できる。
The rotating parts of the four rotating pairs in each of the
このリンク作動装置本体２の構成によれば、基端側のリンクハブ１４に対する先端側のリンクハブ１５の可動範囲を広くとれる。例えば、基端側のリンクハブ１４の中心軸ＱＡ（図２）と先端側のリンクハブ１５の中心軸ＱＢの折れ角θの最大値（最大折れ角）を約±９０°とすることができる。また、基端側のリンクハブ１４に対する先端側のリンクハブ１５の旋回角φを０°〜３６０°の範囲で設定できる。
According to the configuration of the link actuator
図１において、前記複数のアクチュエータ３は、支持部材５の上に円周方向に等配で設置されている。アクチュエータ３の個数は、リンク機構１１，１２，１３と同数の３個である。この実施形態では、アクチュエータ３はモータからなり、その出力軸の回転が減速機構３３（図５）を介してアーム１１ａ〜１３ａに伝達される。
In FIG. 1, the plurality of
各アクチュエータ３（図１）を回転駆動すると、その回転が減速機構３３（図５）を介して軸部１８に伝達されて、基端側のリンクハブ１４（図３）に対する基端側の端部リンク部材であるアーム１１ａ〜１３ａの角度が変更される。それにより、基端側のリンクハブ１４に対する先端側のリンクハブ１５の姿勢が決まる。この姿勢は、折れ角θ（図２）および旋回角φ（図２）により規定される。アーム１１ａ〜１３ａの回転角β１，β２，β３は、回転角検出手段３５により検出された値と減速機構３３の減速比との値から見積もられる。
When each actuator 3 (FIG. 1) is driven to rotate, the rotation is transmitted to the
つぎに、図１と共に制御装置４につき説明する。制御装置４は、基端側のリンクハブ１４に対する先端側のリンクハブ１５の姿勢を、現在の姿勢から、この制御装置４に対する外部の外部指令手段４０から与えられる目標姿勢へ変更するように、各アクチュエータ３を制御する装置である。外部指令手段４０は、このリンク作動装置１を組み込んだ機器（図示せず）や、操作盤（図示せず）等に設けられる。制御装置４は、コンピュータによる数値制御式のものであり、主に姿勢変更制御手段４１により構成される。
なお、姿勢変更制御手段４１は、先端側のリンクハブ１５を、外部指令手段４０から与えられた目標姿勢である最終目標姿勢へ現在の姿勢から姿勢変更させるにつき、通常は、途中で通過させる複数のポイント（ポイント間毎に移動量が異なる）を所定の演算により求めて登録しておき、各登録ポイントへ順次移動させるように、指令を順次行う。以下で言う「現在の姿勢」および「目標姿勢」は、個々のポイント間の移動における移動前のポイントと移動先のポイントのことである。前記複数のポイントを演算により求めて登録するポイント演算手段（図示せず）は、制御装置４における姿勢変更制御手段４１の内部または外部で、後述の指令変換部４２の上位に設けられる。この他に、外部指令手段４０が各ポイントを指令するようにしても良い。
Next, the
Note that the posture change control means 41 normally has a plurality of
姿勢変更制御手段４１は、各アクチュエータ３を、現在姿勢であるときのアーム１１ａ〜１３ａの回転角から目標姿勢となるときのアーム１１ａ〜１３ａの回転角までの指令アーム回転角度だけ駆動する手段である。姿勢変更制御手段４１は、具体的には、各リンク機構１１〜１３のアクチュエータ３をそれぞれ制御するアクチュエータドライバである複数の個別制御部４４と、指令変換部４２と、同期制御部４３とでなる。
The posture change control means 41 is a means for driving each
指令変換部４２は、前記外部指令手段４０から前記ポイント演算手段（図示せず）を介して、または直接に折れ角θと旋回角φとで与えられる目標姿勢の指令Ｂ（θｂ，φｂ）を、各リンク機構１１〜１３のアーム１１ａ〜１３ａの回転角βに変換することを基本の機能とする手段であり、その変換された各アーム１１ａ〜１３ａの回転角β（β１〜β３）と、現在の各アーム１１ａ〜１３ａの回転角αとの差が、それぞれ請求項で言う指令アーム回転角度となる。折れ角θと旋回角φから各アーム１１ａ〜１３ａの回転角βを求める演算は、後述の関係式（１）による逆変換で行われる。
指令変換部４２は、上記基本の機能を持つ手段に加えて、後述の機能を持つ初期パラメータ生成部４７および判断・変更部４８を有する。
指令変換部４２で変換して得られた指令アーム回転角度は、同期制御部４３を介して、各個別制御部４４に与えられる。
The
The
The command arm rotation angle obtained by conversion by the
概要を説明すると、前記指令変換部４２内で初期パラメータ生成、所定の判断・変更をして、各ポイント間の速度や各アクチュエータ（モータ位置）座標を予め計算する。
同期制御部４３は、各アーム１１ａ〜１３ａの回転の同期制御を行わせる手段であり、同期タイミングに合わせて個別制御部（アクチュエータドライバ）４４へ毎時毎の位置指令を送信する。個別制御部４４では同期制御部４３の指令に応じてアクチュエータ３を駆動する。同期制御部４３は、各個別制御部４４へ指定するパラメータおよび位置をそれぞれ設定するパラメータ設定部５２および位置設定部５３を有し、パラメータ設定部５２には、指令変換部４２により初期生成された制御パラメータを、変更したときはその変更した制御パラメータを設定する。位置設定部５３には指令アーム回転角度を設定する。
The outline will be described. Initial parameter generation, predetermined judgment and change are performed in the
The
各個別制御部４４は、対応するアクチュエータ３の動作を、アーム１１ａ〜１３ａの現在角度から目標角度へ回転させるように制御する手段であり、サーボドライバからなる位置制御部４５および指令実行手段４６を有する。同期制御部４３より各制御パラメータ（例えば、加速時間、減速時間、指令速度）、およびアーム指令回転角度に従い、パルス払い出し等によって位置制御部４５へ動作指令を与え、指令実行手段４６でアクチュエータ３を駆動する。位置制御部４５は、その与えられた動作指令と前記回転角検出手段３５の検出値を用いてフィードバック制御を行う。
Each
指令変換部４２の初期パラメータ生成部４７は、上位から与えられた各アーム１１ａ〜１３ａの指令アーム回転角度だけ、各アーム１１ａ〜１３ａが同時に回転を始めて同時に回転が止まるように制御する制御パラメータを、定められた規則に従って制御パラメータの初期値として生成する。この初期値として生成する制御パラメータには、例えば加速時間、減速時間、および指令速度を含み、加速，減速の時定数は一定値として直線加減速制御を行わせる。
初期パラメータ生成部４７は、制御パラメータのうち、加速時間と減速時間を、リンク作動装置の共振周波数の１周期に設定する。ここで言う共振周波数は、先端側のリンクハブ１５にエンドエフェクタ８等の、搭載された全ての搭載物を設置した状態での共振周波数のことである。
The initial
The initial
判断・変更部４８は、初期パラメータ生成部４７で生成された制御パラメータを、定められた基準で判断し、変更すべきと判断した場合に変更する手段である。
判断・変更部４８は、図６に流れ図に示した処理を行う手段であって、図１の動作時間推定手段４９、条件選定手段５０、およびパラメータ変更手段５１を有する。
動作時間推定手段４９は、前記アクチュエータ３の加速時間、減速時間、指令速度、および指令アーム回転角度から動作時間を推定する。
条件選定手段５０は、この推定された推定動作時間と、加速時間および減速時間の合計とを比較する手段である。
パラメータ変更手段５１は、条件選定手段５０による比較の結果により、前記制御パラメータの初期値を変更する手段である。パラメータ変更手段５１によりどの制御パラメータをどのように変更するかは、次の制御動作および作用の説明欄で説明する。
The determination /
The determination /
The operation time estimation means 49 estimates the operation time from the acceleration time, deceleration time, command speed, and command arm rotation angle of the
The condition selection means 50 is means for comparing the estimated estimated operation time with the sum of the acceleration time and the deceleration time.
The
上記構成による制御動作および作用を説明する。制御動作の説明の前に、リンク作動装置１の各部の動作の関係を説明する。制御対象となるリンク作動装置１は、前記折れ角θおよび旋回角φと、各基端側の端部リンク１１ａ，１２ａ，１３ａの回転角βｎ（β１，β２，β３）とが次式（１）で表わされる関係にある。
cos （θ／２）sin βｎ−sin （θ／２）sin （φ＋δｎ）cos βｎ＋sin （γ／２）＝０ …式（１）
ここで、γは、アーム１１ａ，１２ａ，１３ａに回転自在に連結された中央リンク部材１１ｃ，１２ｃ，１３ｃの連結端軸と、先端側の端部リンク部材１１ｂ，１２ｂ，１３ｂに回転自在に連結された中央リンク部材１１ｃ，１２ｃ，１３ｃの連結端軸とが成す角度である。δｎ（δ１，δ２，δ３）（図示せず）は、基準となるアーム１１ａに対する各基端側の端部リンク部材１１ａ，１２ａ，１３ａの円周方向の離間角である。リンク機構１１，１２，１３の数が３組で、各リンク機構１１，１２，１３が円周方向に等配である場合、各アーム１１ａ，１２ａ，１３ａの離間角δ１，δ２，δ３はそれぞれ０°，１２０°，２４０°となる。
The control operation and action of the above configuration will be described. Prior to the description of the control operation, the relationship of the operation of each part of the link actuator 1 will be described. The link actuating device 1 to be controlled has the bending angle θ and the turning angle φ and the rotation angles βn (β1, β2, β3) of the
cos (θ / 2) sin βn−sin (θ / 2) sin (φ + δn) cos βn + sin (γ / 2) = 0 Equation (1)
Here, γ is rotatably connected to the connecting end shafts of the
上記リンクハブとアーム回転角との関係式（１）より、先端側のリンクハブ１５のある姿勢Ａ（θa 、φa ）と姿勢Ｂ（θb 、φb ）について、姿勢Ａ、Ｂに対応するアーム回転角は、それぞれ姿勢Ａのアーム回転角（β１a 、β２a 、β３a ）、姿勢Ｂのアーム回転角（β1b、β2b、β3b）として関係が成り立つ。
From the relational expression (1) between the link hub and the arm rotation angle, for the posture A (θa, φa) and the posture B (θb, φb) with the
姿勢変更の制御を説明する。先端側のリンクハブ１５の姿勢変更は、それぞれのアーム１１ａ〜１３ａが直線加減速制御で同期してＡのアーム回転角からＢのアーム回転角へ駆動することで行われる。ここで図６に制御パラメータを決めるフローチャートを示す。
The attitude change control will be described. The posture of the
概要を説明すると、各アーム１１ａ〜１３ａ毎に、これらアーム１１ａ〜１３ａを駆動するアクチュエータ３の加速時間と減速時間、指令速度、指令アーム回転角度から、動作時間推定手段４９により推定動作時間を得る（Ｓ１）。次に条件選定手段（５０）により、先に推定した動作時間からパラメータ変更の有無を判定する（Ｓ２）。
In brief, for each of the
図７にアーム１１ａ〜１３ａを回転させるアクチュエータ３の推定動作時間Ｔｅと、加速時間Ｔacc 、減速時間Ｔdcc との関係を示す。図７(a) は推定動作時間Ｔｅが加速時間Ｔacc 及び減速時間Ｔdcc の合計よりも長い場合を示し、図７(b) は推定動作時間Ｔｅが加速時間Ｔacc 及び減速時間Ｔdcc の合計よりも短い場合を示す。同図に斜線を施した部分の面積が移動量ｄ（現在姿勢であるときのアームの回転角から目標姿勢となるときのアームの回転角までの指令アーム回転角度）である。条件選定手段５０は、図７に示すような関係からパラメータ変更の有無を判断する（Ｓ２）。
判断の結果、パラメータ変更が必要な場合、パラメータ変更手段５１により制御パラメータを変更する（Ｓ４）。
FIG. 7 shows the relationship between the estimated operation time Te of the
If the parameter change is necessary as a result of the determination, the control parameter is changed by the parameter changing means 51 (S4).
条件選定手段５０において推定動作時間Ｔｅと加速時間Ｔacc 及び減速時間Ｔdcc の合計とを比較し、図８（ａ）のように推定動作時間Ｔｅの方が長い場合は、パラメータ変更手段５１は、図８（ｂ）のようにアーム１１ａ〜１３ａを駆動する指令速度Ｖ１を、初期値である指令速度Ｖ０よりも速くする。このときの指令速度Ｖ１は、変更後の動作時間Ｔｆが加速時間Ｔacc 及び減速時間Ｔdcc の合計未満とならないようにする。また、速度の上限Ｖmax が設定されている場合、図８（ｃ）のように上限速度Ｖmax での台形駆動になる。なお、変更後の動作時間Ｔｆが加速時間Ｔacc 及び減速時間Ｔdcc の和になるようにして変更前と同じ動作量だけ動作させるように変更した場合に、変更後の指令速度Ｖ１が上限速度Ｖmax 達しない場合は、図８（ｂ）と同様の三角形移動になる。
The condition selection means 50 compares the estimated operation time Te with the sum of the acceleration time Tacc and the deceleration time Tdcc. If the estimated operation time Te is longer as shown in FIG. As shown in FIG. 8B, the command speed V1 for driving the
前記条件選定手段５０（図６）で推定動作時間Ｔｅと加速時間Ｔacc 及び減速時間Ｔdcc の合計とを比較したときに（Ｓ２）、図９(a) に示すように推定動作時間Ｔｅの方が短い場合は、図９(b) のように、パラメータ変更手段５１により、加速時間Ｔacc 及び減速時間Ｔdcc の合計が推定動作時間Ｔｅと等しくなる指令速度Ｖ１、または加速度に変更する。なお、ここでは、このように変更する変更規則を、手法（１）となる定められた規則と称する。 When the condition selection means 50 (FIG. 6) compares the estimated operation time Te with the sum of the acceleration time Tacc and the deceleration time Tdcc (S2), the estimated operation time Te is more as shown in FIG. 9 (a). If it is shorter, as shown in FIG. 9 (b), the parameter changing means 51 changes the command speed V1 or the acceleration so that the sum of the acceleration time Tacc and the deceleration time Tdcc becomes equal to the estimated operation time Te. Here, the change rule to be changed in this way is referred to as a predetermined rule as the method (1).
この構成のリンク作動装置の制御装置によると、上記のように条件判定し、制御パラメータを変更するため、リンク姿勢変更時の移動量が小さく、指令速度Ｖ０が速い条件において、自動で指令速度を変更して先端側のリンクハブ部１５の整定時間を短くすることができる。
According to the control device for the link operating device having this configuration, since the condition is determined as described above and the control parameter is changed, the command speed is automatically set under the condition that the movement amount when the link attitude is changed is small and the command speed V0 is fast. This can be changed to shorten the settling time of the
パラメータ変更手段５１による制御パラメータを変更する定められた規則は、上記手法（１）による他に、次の手法（２）〜（６）のいずれかを用いる規則としても良い。
手法（２）では、図９(a) に示すように推定動作時間Ｔｅの方が短い場合に、手法（１）の代わりに、パラメータ変更手段５１により、初期設定された指令速度Ｖ０と移動量ｄから求めた推定動作時間Ｔｅに比例した割合で減速した速度に変更する。
The determined rule for changing the control parameter by the parameter changing means 51 may be a rule using any of the following methods (2) to (6) in addition to the method (1).
In the method (2), when the estimated operation time Te is shorter as shown in FIG. 9 (a), instead of the method (1), the parameter changing means 51 uses the command speed V0 and the movement amount that are initially set. The speed is decelerated at a rate proportional to the estimated operation time Te obtained from d.
手法（３）では、パラメータ変更手段５１により、初期に設定した指令速度Ｖ０を所定の割合で減速した速度に変更する。 In method (3), the parameter changing means 51 changes the initially set command speed V0 to a speed reduced at a predetermined rate.
手法（４）では、パラメータ変更手段５１により、直線加減速制御からＳ字加減速制御に変更する。Ｓ字加減速制御は、直線加減速制御における加速および減速を、直線に代えてＳ字状の速度曲線となるように、時定数を変えながら加減速する制御である。
なお、時定数を変える場合、各個別制御部４４は、設定された時定数を用いて制御を行う構成とする。
手法（５）では、パラメータ変更手段５１により、直線加減速制御から指数関数型の加減速制御に変更する。指数関数型の加減速制御は、直線加減速制御における加速および減速を、直線に代えて指数関数型の速度曲線となるように、時定数を変えながら加減速する制御である。
Ｓ字加減速制御や指数関数型の加減速制御であると、制御は複雑となるが、加減速を急加速度で行うようにしても、加減速の完了時における整定時間がより一層短縮される。
前記手法（４）と手法（５）については、加減速制御の時定数を推定動作時間Ｔｅに比例した設定割合で設定した時定数としてもよい。
In method (4), the parameter changing means 51 changes the linear acceleration / deceleration control to the S-curve acceleration / deceleration control. The S-curve acceleration / deceleration control is a control for accelerating / decelerating the acceleration and deceleration in the linear acceleration / deceleration control while changing the time constant so as to form an S-shaped speed curve instead of a straight line.
When changing the time constant, each
In method (5), the parameter changing means 51 changes the linear acceleration / deceleration control to exponential acceleration / deceleration control. The exponential function type acceleration / deceleration control is a control that accelerates and decelerates while changing the time constant so that acceleration and deceleration in the linear acceleration / deceleration control become an exponential function type speed curve instead of a straight line.
In the case of S-curve acceleration / deceleration control or exponential function acceleration / deceleration control, the control is complicated, but even if acceleration / deceleration is performed at a rapid acceleration, the settling time at the completion of acceleration / deceleration is further reduced. .
In the methods (4) and (5), the time constant of acceleration / deceleration control may be a time constant set at a set ratio proportional to the estimated operation time Te.
手法（６）では、パラメータ変更手段５１により、加速時間Ｔacc 及び減速時間Ｔdcc を変更する。なお、前記手法（２）〜（６）については、特に変更を記載した制御パラメータ以外の他の制御パラメータは初期値のままとする。 In the method (6), the parameter change means 51 changes the acceleration time Tacc and the deceleration time Tdcc. For the methods (2) to (6), the control parameters other than the control parameters in which the change is described are kept at the initial values.
前記条件選定手段５０（ステップＳ２）において、制御パラメータを変更する条件は、アーム１１ａ〜１３ａの推定動作時間Ｔｅが、加速時間Ｔacc 及び減速時間Ｔdcc との合計以下で、かつ所定の動作時間以上の場合としてもよい。例えば閾値となる所定の動作時間を３０msec、加速時間を４０msec、減速時間を４０msecと設定し、移動量や速度等の諸条件によりアーム１１ａ〜１３ａの推定動作時間が２０msecとなる場合では制御パラメータは変更せず、初期値のパラメータで駆動する。ここで閾値となる所定の動作時間はリンク作動装置１の機械的特性により決定され、推定動作時間Ｔｅをゼロから徐々に増加させ、位置決め完了後のリンク先端の整定時間の増加が見られる時の時間とする。
In the condition selection means 50 (step S2), the condition for changing the control parameter is that the estimated operation time Te of the
また、条件選定手段５０およびパラメータ変更手段５１における扱う速度に関する制御パラメータを、速度の代わりに加速度または減速度とし、加速時間Ｔacc および減速時間Ｔdcc が共振周波数の一周期になるように制御しても良い。
Also, the control parameter relating to the speed handled by the
なお、上記実施形態では、個々のアーム１１ａ〜１３ａ毎に制御パラメータの変更を説明したが、３つのアクチュエータ３を例に、相互の同期につき説明する。３つのアクチュエータ３の各速度をＶ１、Ｖ２、Ｖ３とすると３つのアクチュエータ３の合成速度ＶはＶ＝√(Ｖ１×Ｖ１＋Ｖ２×Ｖ２＋Ｖ３×Ｖ３)となり、Ｖ１、Ｖ２、Ｖ３は各アクチュエータの移動量Ｌ１、Ｌ２、Ｌ３に比例した値になる。今回、ある１軸について変更する速度を計算するが、他の２軸についても距離に応じて同じ割合で速度変更するため、同期は崩れない。
In the above embodiment, the change of the control parameter has been described for each of the
上記実施形態のリンク作動装置の制御装置につき、要点を纏め直して以下に説明する。この実施形態は、リンク作動装置１を高速動作で位置決め時の先端側リンクハブ１５の振動抑制技術である。主に、リンク駆動時の加速時間と減速時間の合計がリンク作動装置１の共振周波数の１周期付近になるように制御することで振動を抑制できることを利用する。移動距離が微小の場合、ある指令速度で駆動すると、駆動時間が共振周波数の１周期以下で駆動（この場合、加速した後減速する三角駆動のため、駆動時間＝加速時間＋減速時間となる）し、先端側リンクハブ１５が位置決め完了後に大きく振動する。この実施形態では、移動距離が微小の場合、予め設定された速度よりも速度を低下させて駆動し、駆動時間（加速時間と減速時間の合計）が共振周波数の１周期付近になるようにして、位置決め完了後の先端側リンクハブ１５の振動を抑制するものである。一般にこの装置は、複数のポイント（ポイント毎に移動量が異なる）を登録し、連続して登録ポイントへ移動して作業をする。
これより移動量が大きく加速・減速時間が十分にとれる（台形駆動）ポイント間動作では指令速度で駆動し、微小移動量のポイント間のみ速度を落とし、設定した加速時間と減速時間が十分確保できる駆動パターンで動作することで、先端の振動を抑制しながら、複数設定したポイント間をできるだけ高速で駆動することができる。
About the control apparatus of the link operating device of the said embodiment, it summarizes and demonstrates the following points. This embodiment is a technique for suppressing vibration of the distal end
Larger travel than this, acceleration / deceleration time is sufficient (trapezoidal drive) Drives at the command speed in the point-to-point operation, and the speed is reduced only between the minute movement distance points, so that the set acceleration time and deceleration time can be secured sufficiently By operating with the drive pattern, it is possible to drive between a plurality of set points as fast as possible while suppressing vibration at the tip.
１…リンク作動装置
３…アクチュエータ
５…基端側の支持部材
７…先端取付部材
８…エンドエフェクタ
１４…基端側のリンクハブ
１５…先端側のリンクハブ
１１〜１３…リンク機構
１１ａ，１２ａ，１３ａ…基端側の端部リンク部材（アーム）
１１ｂ，１２ｂ，１３ｂ…先端側の端部リンク部材
１１ｃ，１２ｃ，１３ｃ…中央リンク部材
３３…減速機
４１…姿勢変更制御手段
４２…指令変換部
４３…同期制御部
４４…個別制御部
４７…初期パラメータ生成部
４８…判断・変更部
５０…条件選定手段
５１…パラメータ変更手段
DESCRIPTION OF SYMBOLS 1 ...
11b, 12b, 13b...
前記基本構成において、前記パラメータ変更手段５１は、前記条件選定手段５０による比較の結果、前記推定動作時間Ｔｅが前記加速時間Ｔacc および減速時間Ｔdcc の合計以下であって、かつ前記推定動作時間Ｔｅがある所定の時間範囲にある場合に前記制御パラメータの変更を行うようにしても良い。例えば、前記指令速度を低下させる。このように指令速度を低下させることにより、加速時間Ｔaccと減速時間Ｔdccをリンク作動装置が持つ共振周波数の１周期付近に設定すれば整定時間が短くなり、通常制御ではリンク先端の振動が大きく整定時間が長くなる場合と比較して、この整定時間を含むリンク姿勢変更動作時間を短縮することができる。なお、駆動条件によっては加速時間と減速時間が共振周波数の１周期でなくても振動が大きくならない場合がある。例えば、低速時
や１周期近辺等。よって、パラメータ変更条件は所定の時間範囲内とすることが好ましい。
In the basic configuration, as a result of the comparison by the
この発明の制御装置において、前記推定動作時間が加速時間と減速時間との合計よりも小さい場合、前記パラメータ変更手段５１は、前記制御パラメータのうちの前記指令速度を、前記アクチュエータ３の動作が所定の推定動作時間Ｔｅとなるような指令速度に低下させる。
この場合、リンク先端の振動を抑制でき、整定時間を含む動作時間を短縮できるという効果が得られる。
この場合に、前記所定の推定動作時間Ｔｅは、加速時間Ｔacc と減速時間Ｔdcc の合計としても良い。
一般に移動距離が短い場合、モータは加速した後、中間地点で減速に切替わる。動作時間は加速時間と減速時間の合計となり、この合計時間がリンク装置の共振周波数の２周期分から大きく乖離するとリンク先端は位置決め完了後に大きく振動する。しかし、振動を抑制する動作時間（加速時間と減速時間の合計）をリンク装置の共振周波数の２周期に合わせると振動は抑制できる。
In the control device of the present invention, when the estimated operation time is smaller than the sum of the acceleration time and the deceleration time, the parameter changing means 51 determines the command speed of the control parameters, and the operation of the
In this case, it is possible to suppress the vibration at the link tip and to shorten the operation time including the settling time.
In this case, the predetermined estimated operation time Te may be the sum of the acceleration time Tacc and the deceleration time Tdcc.
In general, when the moving distance is short, the motor accelerates and then switches to deceleration at an intermediate point. The operation time is the sum of the acceleration time and the deceleration time. If this total time deviates greatly from two periods of the resonance frequency of the link device, the link tip vibrates greatly after the positioning is completed. However, the vibration can be suppressed by adjusting the operation time for suppressing the vibration (the sum of the acceleration time and the deceleration time) to two periods of the resonance frequency of the link device.
この発明の制御装置において、前記パラメータ変更手段５１は、制御パラメータの変更により、前記姿勢変更制御手段４１による直線加減速制御をＳ字加減速制御に変更するようにしても良い。
Ｓ字加減速制御であると、制御は複雑となるが、加減速を急加速度で行うようにしても、加減速の完了時における整定時間がより一層短縮される。
In the control apparatus of the present invention, the
In the S-curve acceleration / deceleration control, the control is complicated, but even if acceleration / deceleration is performed at a rapid acceleration, the settling time at the completion of acceleration / deceleration is further shortened.
この発明の制御装置において、前記パラメータ変更手段５１は、制御パラメータの変更により、前記姿勢変更制御手段による直線加減速制御を指数関数型の加減速制御にするようにしても良い。
指数関数型の加減速制御にした場合も、加減速を急加速度で行うようにしても、加減速の完了時における整定時間がより一層短縮される。
In the control apparatus of the present invention, the
Even in the case of exponential acceleration / deceleration control, even if acceleration / deceleration is performed at a rapid acceleration, the settling time at the completion of acceleration / deceleration is further reduced.
この発明の制御装置４において、前記パラメータ変更手段５１は、前記指令速度の代わりに加減速度を変更して各条件を満たす制御を行うようにしても良い。なお、この発明で必要なことは加速時間と減速時間が共振周波数の１周期となるように制御して、位置決め完了後のリンク先端の振動を抑制することである。そのための速度パラメータは加速・減速時間と（到達）速度、加速度・減速度と（到達）速度のいずれであっても問題がない。
In the
上記実施形態のリンク作動装置の制御装置につき、要点を纏め直して以下に説明する。この実施形態は、リンク作動装置１を高速動作で位置決め時の先端側リンクハブ１５の振動抑制技術である。主に、リンク駆動時の加速時間と減速時間がリンク作動装置１の共振周波数の１周期付近になるように制御することで振動を抑制できることを利用する。移動距離が微小の場合、ある設定速度で駆動すると、駆動時間が共振周波数の２周期以下で駆動（この場合、加速した後減速する三角駆動のため、駆動時間＝加速時間＋減速時間となる）し、先端側リンクハブ１５が位置決め完了後に大きく振動する。この実施形態では、移動距離が微小の場合、予め設定された速度よりも速度を低下させて駆動し、駆動時間（加速時間と減速時間の合計）が共振周波数の２周期付近になるようにして、位置決め完了後の先端側リンクハブ１５の振動を抑制するものである。一般にこの装置は、複数のポイント（ポイント毎に移動量が異なる）を登録し、連続して登録ポイントへ移動して作業をする。これより移動量が大きく加速・減速時間が十分にとれる（台形駆動）ポイント間動作では設定速度で駆動し、微小移動量のポイント間のみ速度を落とし、設定した加速時間と減速時間が十分確保できる駆動パターンで動作することで、先端の振動を抑制しながら、複数設定したポイント間をできるだけ高速で駆動することができる。
About the control apparatus of the link operating device of the said embodiment, it summarizes and demonstrates the following points. This embodiment is a technique for suppressing vibration of the distal end
Claims (11)
前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を、現在姿勢から指令された目標姿勢へ変更させるように、前記各アクチュエータを制御するリンク作動装置の制御装置であって、
前記各アクチュエータを、前記現在姿勢であるときの前記アームの回転角から前記目標姿勢となるときの前記アームの回転角までの指令アーム回転角度だけ、アーム回転制御の制御パラメータである加速時間、減速時間、および指令速度に従って、互いに同期して駆動する姿勢変更制御手段を備え、
前記制御パラメータのうち、初期パラメータとして加速時間、減速時間を設定し、
前記アクチュエータの加速時間、減速時間、指令速度、および指令アーム回転角度から動作時間を推定する動作時間推定手段と、
この推定された推定動作時間と加速時間および減速時間の合計とを比較する条件選定手段と、
この条件選定手段による比較の結果に応じて定められた規則に従い前記制御パラメータを変更するパラメータ変更手段とを設けた、
ことを特徴とするリンク作動装置の制御装置。 The link hub on the distal end side is connected to the link hub on the proximal end side through three or more sets of link mechanisms in such a manner that the posture can be changed, and each of the link mechanisms includes a link hub on the proximal end side and a link hub on the distal end side. End link members on the base end side and the tip end side, one end of which is rotatably connected to the link hub, and a center on which both ends are rotatably connected to the other ends of the end link members on the base end side and the tip end side, respectively. Each of the link mechanisms is a geometric model in which the link mechanism is represented by a straight line, and the base end side portion and the tip end side portion are symmetrical with respect to the center portion of the center link member, The posture of the distal end side link hub with respect to the proximal end side link hub is arbitrarily changed by rotating an arm which is the proximal end side end link member to each of the three or more sets of link mechanisms. In link actuator provided with an actuator,
A control device for a link actuator that controls each actuator so as to change the posture of the distal end side link hub with respect to the proximal end side link hub from a current posture to a commanded target posture;
For each actuator, an acceleration time and deceleration that are control parameters for arm rotation control by the command arm rotation angle from the arm rotation angle at the current posture to the arm rotation angle at the target posture. In accordance with time and command speed, equipped with posture change control means that drive in synchronization with each other,
Among the control parameters, set acceleration time and deceleration time as initial parameters,
An operation time estimating means for estimating an operation time from an acceleration time, a deceleration time, a command speed, and a command arm rotation angle of the actuator;
A condition selecting means for comparing the estimated estimated operation time with the sum of the acceleration time and the deceleration time;
Parameter changing means for changing the control parameter according to a rule determined according to the result of comparison by the condition selecting means,
A control device for a link actuating device.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2017180835A JP6517292B2 (en) | 2017-09-21 | 2017-09-21 | Control device of link operating device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2017180835A JP6517292B2 (en) | 2017-09-21 | 2017-09-21 | Control device of link operating device |
Related Parent Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2013187016A Division JP6215623B2 (en) | 2013-09-10 | 2013-09-10 | Link actuator control device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2017228318A true JP2017228318A (en) | 2017-12-28 |
| JP6517292B2 JP6517292B2 (en) | 2019-05-22 |
Family
ID=60892061
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2017180835A Active JP6517292B2 (en) | 2017-09-21 | 2017-09-21 | Control device of link operating device |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP6517292B2 (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108170170A (en) * | 2018-01-15 | 2018-06-15 | 中国工程物理研究院机械制造工艺研究所 | A kind of mobile sandwich type element pose adjustment and driving device |
| CN114035513A (en) * | 2021-09-28 | 2022-02-11 | 苏州谋迅智能科技有限公司 | S-shaped speed curve look-ahead planning method and device, storage medium and computing device |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH05341847A (en) * | 1992-06-09 | 1993-12-24 | Fuji Electric Co Ltd | Function generating device for positioning control |
| JPH08126980A (en) * | 1994-11-02 | 1996-05-21 | Toshiba Corp | Robot control method and its device |
| JPH10262386A (en) * | 1997-03-18 | 1998-09-29 | Matsushita Electric Ind Co Ltd | Method for controlling motor |
| JP2013121652A (en) * | 2011-11-07 | 2013-06-20 | Ntn Corp | Parallel link mechanism, constant velocity universal joint and link actuator |
-
2017
- 2017-09-21 JP JP2017180835A patent/JP6517292B2/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH05341847A (en) * | 1992-06-09 | 1993-12-24 | Fuji Electric Co Ltd | Function generating device for positioning control |
| JPH08126980A (en) * | 1994-11-02 | 1996-05-21 | Toshiba Corp | Robot control method and its device |
| JPH10262386A (en) * | 1997-03-18 | 1998-09-29 | Matsushita Electric Ind Co Ltd | Method for controlling motor |
| JP2013121652A (en) * | 2011-11-07 | 2013-06-20 | Ntn Corp | Parallel link mechanism, constant velocity universal joint and link actuator |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108170170A (en) * | 2018-01-15 | 2018-06-15 | 中国工程物理研究院机械制造工艺研究所 | A kind of mobile sandwich type element pose adjustment and driving device |
| CN108170170B (en) * | 2018-01-15 | 2023-07-07 | 中国工程物理研究院机械制造工艺研究所 | Movable shell pose adjusting and driving device |
| CN114035513A (en) * | 2021-09-28 | 2022-02-11 | 苏州谋迅智能科技有限公司 | S-shaped speed curve look-ahead planning method and device, storage medium and computing device |
| CN114035513B (en) * | 2021-09-28 | 2024-07-02 | 苏州谋迅智能科技有限公司 | S-shaped speed curve look-ahead planning method and device, storage medium and computing equipment |
Also Published As
| Publication number | Publication date |
|---|---|
| JP6517292B2 (en) | 2019-05-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10065310B2 (en) | Device and method for controlling link actuation device | |
| CN110505945B (en) | Material handling robot with multiple end effectors | |
| US10443693B2 (en) | Operation device for link actuating device | |
| KR20140005771A (en) | Robot control apparatus and robot control method | |
| JP5864322B2 (en) | Method for controlling link operating device and control device therefor | |
| JP4289506B2 (en) | 4-DOF parallel robot | |
| WO2017043420A1 (en) | Composite work device using link operating device | |
| JP2020073298A (en) | Movable hardstop for robotic component | |
| WO2013069533A1 (en) | Link operation device | |
| JP6215623B2 (en) | Link actuator control device | |
| JP7336215B2 (en) | Robot system, control method, article manufacturing method, program, and recording medium | |
| JP2014217913A (en) | Operation teaching method of parallel link robot and parallel link robot | |
| JP2017228318A (en) | Control device for link operation device | |
| CN105189051A (en) | Linking apparatus control device | |
| JP2017061004A (en) | Combination type link operation device | |
| WO2019163997A1 (en) | Working device using parallel link mechanism and control method thereof | |
| JP2020506815A (en) | robot | |
| JP2010076058A (en) | Control device of multiple point manipulator and method for generating operation track of hand for multiple point manipulator | |
| JP6165322B2 (en) | Robot control apparatus and robot control method | |
| JP2010110878A (en) | Articulated robot device and method for controlling the same | |
| JP6498738B1 (en) | Link actuator | |
| JP5976580B2 (en) | Link actuator control device | |
| WO2024048561A1 (en) | Control device for link actuation device | |
| YAMAKAWA | Brachiation motion by a 2-DOF brachiating robot with hook-shaped end effectors | |
| JP7443013B2 (en) | robot control device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20171019 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20171115 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20180822 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20180828 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20181025 |
|
| 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: 20190402 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20190417 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 6517292 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 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |