JP3338540B2 - Trajectory control device for articulated work machines - Google Patents
Trajectory control device for articulated work machinesInfo
- Publication number
- JP3338540B2 JP3338540B2 JP33688093A JP33688093A JP3338540B2 JP 3338540 B2 JP3338540 B2 JP 3338540B2 JP 33688093 A JP33688093 A JP 33688093A JP 33688093 A JP33688093 A JP 33688093A JP 3338540 B2 JP3338540 B2 JP 3338540B2
- Authority
- JP
- Japan
- Prior art keywords
- arm
- control
- trajectory
- arms
- speed command
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Landscapes
- Operation Control Of Excavators (AREA)
- Numerical Control (AREA)
Description
【0001】[0001]
【産業上の利用分野】本発明は、3本以上のアームを有
する多関節作業機の軌跡制御装置に関する。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a trajectory control device for an articulated working machine having three or more arms.
【0002】[0002]
【従来の技術】3本以上のアームを有する多関節作業機
で軌跡制御を行う場合、駆動アームを2本選択し、残り
のアームは固定する方式がとられる。駆動すべき2本の
アームを選択する方式として特開平2−287804号
公報に開示されている方法が知られている。この方法
は、駆動アームとして、アームを回動させたときに軌跡
制御対象部位がX(水平)方向に最大となりY(垂直)
方向に最小となるアームと、Y方向に最大となりX方向
に最小となるアームを選択する。そして、それぞれのア
ームについて制御方向に対してストロークエンドか否か
の判定を行なっている。2. Description of the Related Art When performing trajectory control with an articulated working machine having three or more arms, a method is adopted in which two drive arms are selected and the remaining arms are fixed. As a method for selecting two arms to be driven, a method disclosed in JP-A-2-287804 is known. According to this method, as the drive arm, when the arm is rotated, the trajectory control target portion becomes maximum in the X (horizontal) direction and Y (vertical)
The arm that has the minimum in the direction and the arm that has the maximum in the Y direction and has the minimum in the X direction are selected. Then, it is determined whether or not each arm is at the stroke end in the control direction.
【0003】たとえば、垂直下げの軌跡制御を行なう場
合、図11(a)に示すような姿勢のときは第3アーム
4がY軸方向の制御に最も適したアームとして、第1ア
ーム1がX軸方向の制御に最も適したアームとして選択
される。アームの回動方向は作業方向がY軸方向となっ
ているので第3アーム3が時計回りに回動する。第1ア
ーム1は第3アーム3の回動により生じる軌跡制御位置
からのずれを補正する動きをするため、角度A34に応
じて時計回りあるいは反時計回り方向に回動する。For example, when performing a trajectory control of vertical lowering, when the posture is as shown in FIG. 11A, the third arm 4 is the most suitable arm for the control in the Y-axis direction, and the first arm 1 is the X arm. It is selected as the arm most suitable for axial control. Since the working direction of the arm is the Y-axis direction, the third arm 3 rotates clockwise. The first arm 1 rotates clockwise or counterclockwise in accordance with the angle A34 in order to perform a movement for correcting a deviation from the trajectory control position caused by the rotation of the third arm 3.
【0004】さらに、直線軌跡制御を行なう場合とし
て、特開平1−278623号公報に開示されている方
法が知られている。この従来技術では、Y方向を作業方
向にX方向を修正方向にしている。アーム先端を作業方
向に軌跡制御するときの修正方向のずれ量(偏差Δx)
を検出し、そのずれ量と速度指令レバーにより与えられ
る作業方向の速度指令値YVとから偏差修正方向の速度
指令値XVを求める。そして、アーム先端が予め定めた
軌跡の方向に運動するように両方向の速度指令値により
2つのアームを駆動するようにしたものである。Further, a method disclosed in Japanese Patent Application Laid-Open No. 1-278623 is known as a method of performing the linear trajectory control. In this prior art, the Y direction is the working direction and the X direction is the correction direction. Amount of deviation (deviation Δx) in the correction direction when controlling the trajectory of the arm tip in the working direction
Is detected, and a speed command value XV in the deviation correction direction is obtained from the shift amount and the speed command value YV in the working direction given by the speed command lever. The two arms are driven by speed command values in both directions so that the tip of the arm moves in the direction of a predetermined locus.
【0005】[0005]
【発明が解決しようとする課題】しかしながら上記2つ
の方式を組合せて使用する場合につぎのような問題点が
ある。図11(b)のような状態で第1アーム1が反時
計回り方向でストロークエンドだったとすると、第3ア
ーム3と第1アーム1での組合わせでは偏差フィードバ
ック制御ができないという問題がある。However, when the above two methods are used in combination, there are the following problems. If the first arm 1 has reached the stroke end in the counterclockwise direction in the state as shown in FIG. 11B, there is a problem that the deviation feedback control cannot be performed with the combination of the third arm 3 and the first arm 1.
【0006】本発明の目的は、偏差フィードバックを加
えた軌跡制御において最適な一対のアームを選択して軌
跡制御不能状態を極力少なくするようにした多関節作業
機の軌跡制御装置を提供することにある。It is an object of the present invention to provide a trajectory control device for an articulated working machine in which an optimum pair of arms is selected in trajectory control to which deviation feedback is added, so that the trajectory control disabled state is minimized. is there.
【0007】[0007]
【課題を解決するための手段】一実施例を示す図1およ
び図4に対応づけて説明すると、本発明は、関節を介し
て回動可能に連結された3本以上のアームと、それらの
アームを駆動するアクチュエータと、各アームの姿勢を
検出する姿勢検出手段100と、軌跡対象部位の目標軌
跡からのずれ量を演算するずれ量演算手段604と、軌
跡制御対象部位の目標軌跡に沿う作業方向の作業速度を
指令する速度指令手段200と、ずれ量と前記作業速度
とに基づいて目標軌跡に直交する修正方向の修正方向速
度指令値を演算する修正方向速度指令値演算手段600
と、入力信号に応じて2本のアームを選択し、選択した
2本のアームに応じた拘束条件を出力する拘束条件選択
手段300(300A)と、前記拘束条件と検出された
アーム姿勢と作業速度指令値と修正方向速度指令値とに
基づいて、軌跡制御対象部位が目標軌跡上を移動するよ
うに、選択されたアームの回動角速度を演算するアーム
角速度制御値演算手段400と、演算されたアーム角速
度でアームが回動するように前記アクチュエータを駆動
する駆動制御値演算手段600とを具備する多関節作業
機の軌跡制御装置に適用される。そして、上述した目的
は、上記拘束条件選択手段300において、前記作業方
向の移動量が最大となり、前記修正方向には最小となる
アームを選択するとともに、修正方向には最大となり作
業方向には最小となるアームであって、いずれかの回動
方向がストロークエンドではないアームを選択し、選択
した2本のアームに応じた拘束条件を出力することによ
り達成される。また、上記拘束条件選択手段300Aに
おいて、前記作業方向の移動量が最大となり、前記修正
方向には最小となるアームを選択するとともに、修正方
向には最大となり作業方向には最小となるアームであっ
て、前記ずれ量によるフィードバック制御を行なわない
場合のアーム回動方向とフィードバック制御を行なう場
合のアーム回動方向とが同一である場合にはその回動方
向でストロークエンドでないアームを選択し、選択した
2本のアームに応じた拘束条件を出力するようにして
も、上記目的を達成できる。DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Referring to FIGS. 1 and 4 showing one embodiment, the present invention comprises three or more arms rotatably connected via joints and their arms. Actuator for driving the arm, attitude detecting means 100 for detecting the attitude of each arm, shift amount calculating means 604 for calculating the shift amount of the locus target portion from the target locus, and work along the target locus of the locus control target portion Speed command means 200 for commanding the working speed in the direction, and correction direction speed command value calculating means 600 for calculating a correction direction speed command value in a correction direction orthogonal to the target trajectory based on the deviation amount and the working speed.
A constraint condition selecting means 300 (300A) for selecting two arms in accordance with an input signal and outputting a constraint condition according to the selected two arms; Arm angular speed control value calculating means 400 for calculating the rotation angular speed of the selected arm based on the speed command value and the corrected direction speed command value so that the trajectory control target moves on the target trajectory; The present invention is applied to a trajectory control device of an articulated working machine, comprising: a drive control value calculating means 600 for driving the actuator so that the arm rotates at the arm angular velocity. The object described above is to select the arm in which the movement amount in the working direction is the largest and the arm is the smallest in the correction direction, and is the largest in the correction direction and the smallest in the working direction. This is achieved by selecting an arm whose rotation direction is not the stroke end and outputting a constraint condition according to the selected two arms. In the constraint condition selecting means 300A, the arm whose movement amount in the working direction is maximum and which is the smallest in the correction direction is selected, and the arm which is largest in the correction direction and is the smallest in the working direction. If the arm rotation direction in the case where the feedback control based on the deviation amount is not performed is the same as the arm rotation direction in the case where the feedback control is performed, an arm that is not a stroke end in the rotation direction is selected. The above object can also be achieved by outputting the constraint conditions according to the two arms.
【0008】[0008]
【作用】拘束条件選択手段300は、移動量が修正方向
には最大となり作業方向には最小となる修正方向側アー
ムを選択する場合、いずれかの回動方向がストロークエ
ンドでない修正方向側アームを選択し、その拘束条件を
各速度演算手段400に入力する。修正方向速度指令値
演算手段600は、軌跡制御対象部位と目標軌跡との間
の偏差に応じて修正方向速度指令値を演算し、その修正
方向速度指令値と、作業方向速度指令値が角速度指令値
演算手段400に入力される。そしてその演算結果が制
御手段500に入力されて一対のアームが演算された角
速度で駆動される。上記拘束条件選択手段300Aは、
移動量が修正方向には最大となり作業方向には最小とな
る修正方向側アームを選択する場合、前記ずれ量による
フィードバック制御を行なわない場合のアーム回動方向
とずれ量によるフィードバック制御を行なう場合のアー
ム回動方向とが同一であれば、その方向でストロークエ
ンドでないアームを選択する。When selecting a correction direction side arm whose movement amount is maximum in the correction direction and minimum in the work direction, the constraint condition selection means 300 selects the correction direction side arm whose rotation direction is not the stroke end. The selected constraint condition is input to each speed calculating means 400. The correction direction speed command value calculating means 600 calculates a correction direction speed command value in accordance with the deviation between the trajectory control target portion and the target trajectory, and calculates the correction direction speed command value and the work direction speed command value as angular speed commands. It is input to the value calculation means 400. The calculation result is input to the control means 500, and the pair of arms are driven at the calculated angular velocity. The constraint condition selecting means 300A includes:
When selecting a correction direction side arm in which the movement amount is maximum in the correction direction and minimum in the work direction, when the feedback control based on the arm rotation direction and the deviation amount is performed when the feedback control based on the deviation amount is not performed. If the arm rotation direction is the same, an arm that does not have a stroke end in that direction is selected.
【0009】なお、本発明の構成を説明する上記課題を
解決するための手段と作用の項では、本発明を分かり易
くするために実施例の図を用いたが、これにより本発明
が実施例に限定されるものではない。In the means and means for solving the above problems which explain the constitution of the present invention, the drawings of the embodiments are used to make the present invention easy to understand. However, the present invention is not limited to this.
【0010】[0010]
−第1の実施例− 図1〜図6により、図7に示す4本アーム式作業機に本
発明を適用する場合について説明する。図7において、
下部走行体LT上に旋回可能に上部旋回体USが設けら
れ、これらにより作業機本体CMが構成される。上部旋
回体USには第1アーム1が回動可能に設けられ、その
先端に第2アーム2が回動可能に設けられ、その先端に
第3アーム3が回動可能に設けられ、その先端に第4ア
ーム4が回動可能に設けられており、各アーム1〜4は
それぞれ油圧シリンダ5〜8により駆動される。第4ア
ーム4の先端には作業用アタッチメント、例えばアース
オーガ掘削ユニット9がピン結合される。図10に示す
ように、第4アーム4に代えて例えば掘削バケット等の
作業用アタッチメントを取付け、油圧シリンダ8により
回動可能としても良い。この場合は作業用アタッチメン
トATの姿勢角一定制御が行われる。First Embodiment A case where the present invention is applied to the four-arm working machine shown in FIG. 7 will be described with reference to FIGS. In FIG.
An upper revolving unit US is provided rotatably on the lower traveling unit LT, and these constitute a working machine body CM. A first arm 1 is rotatably provided on the upper revolving superstructure US, a second arm 2 is rotatably provided at the tip thereof, and a third arm 3 is rotatably provided at the tip thereof. A fourth arm 4 is rotatably provided, and the arms 1 to 4 are driven by hydraulic cylinders 5 to 8, respectively. A working attachment, for example, an earth auger excavating unit 9 is pin-connected to the tip of the fourth arm 4. As shown in FIG. 10, a work attachment such as a digging bucket may be attached instead of the fourth arm 4, and the work attachment may be rotatable by the hydraulic cylinder 8. In this case, constant attitude angle control of the work attachment AT is performed.
【0011】図2(a)のようにこの作業機の座標を定
義し、以下の説明はこの座標に従う。図2(a)に示す
ように、点Oを第1アーム1の回動支点とし、第1〜第
4のアーム1〜4の長さをL1〜L4、相対角度をT1〜
T4、各アーム先端の座標を(X1,Y1)、(X2,Y
2)、(X3,Y3)、(X4,Y4)とするとき、各座標
はそれぞれ、The coordinates of the working machine are defined as shown in FIG. 2A, and the following description will follow these coordinates. As shown in FIG. 2 (a), the point O is a pivot point of the first arm 1, the lengths of the first to fourth arms 1-4 are L1 to L4, and the relative angles are T1 to L4.
T4, the coordinates of the tip of each arm are (X1, Y1), (X2, Y
2), (X3, Y3), (X4, Y4), each coordinate is
【0012】[0012]
【数1】 X1=L1cosT1 …(1) Y1=L1sinT1 …(2)X1 = L1cosT1 (1) Y1 = L1sinT1 (2)
【数2】 X2=X1+L2cos(T1-T2) …(3) Y2=Y1+L2sin(T1-T2) …(4)X2 = X1 + L2cos (T1-T2) (3) Y2 = Y1 + L2sin (T1-T2) (4)
【数3】 X3=X2+L3cos(T1-T2-T3) …(5) Y3=Y2+L3sin(T1-T2-T3) …(6)X3 = X2 + L3cos (T1-T2-T3) (5) Y3 = Y2 + L3sin (T1-T2-T3) (6)
【数4】 X4=X3+L4cos(T1-T2-T3-T4) …(7) Y4=Y3+L4sin(T1-T2-T3-T4) …(8) と表すことができる。したがって、第4のアーム4先端
の速度X4V,Y4Vは、X4 = X3 + L4cos (T1-T2-T3-T4) (7) Y4 = Y3 + L4sin (T1-T2-T3-T4) (8) Therefore, the speeds X4V and Y4V of the tip of the fourth arm 4 are:
【0013】[0013]
【数5】 X4V=−T1V・L1sinT1 −(T1V-T2V)・L2sin(T1-T2) −(T1V-T2V-T3)・L3sin(T1-T2-T3) −(T1V-T2V-T3V-T4V)・L4sin(T1-T2-T3-T4) …(9) ただし、T1V,T2V,T3V,T4Vはそれぞれ角速度を表
わす。X4V = −T1V · L1sinT1− (T1V−T2V) · L2sin (T1−T2) − (T1V−T2V−T3) ・ L3sin (T1−T2−T3) − (T1V−T2V−T3V−T4V) L4sin (T1-T2-T3-T4) (9) where T1V, T2V, T3V and T4V each represent an angular velocity.
【数6】 Y4V=T1V・L1cosT1 +(T1V-T2V)・L2cos(T1-T2) +(T1V-T2V-T3V)・L3cos(T1-T2-T3) +(T1V-T2V-T3V-T4V)・L4cos(T1-T2-T3-T4) …(10) と表すことができる。Y4V = T1V · L1cosT1 + (T1V−T2V) · L2cos (T1−T2) + (T1V−T2V−T3V) · L3cos (T1−T2−T3) + (T1V−T2V−T3V−T4V) ・L4cos (T1-T2-T3-T4) (10)
【0014】周知のとおり4本のアームを有する多関節
作業機においては、アーム先端を軌跡制御するには拘束
条件を定めて2本のアームを駆動するが、その組合せは
次に示すように6通りある。 (1)第3,第4のアーム3,4を固定 (第1,第2のアーム1,2で駆動) (2)第2,第4のアーム2,4を固定 (第1,第3のアーム1,3で駆動) (3)第2,第3のアーム3,3を固定 (第1,第4のアーム1,4で駆動) (4)第1,第4のアーム1,4を固定 (第2,第3のアーム2,3で駆動) (5)第1,第3のアーム1,3を固定 (第2,第4のアーム2,4で駆動) (6)第1,第2のアーム1,2を固定 (第3,第4のアーム3,4で駆動)As is well known, in a multi-joint working machine having four arms, to control the trajectory of the arm tip, two arms are driven by defining a constraint condition. There is a street. (1) Fix third and fourth arms 3 and 4 (driven by first and second arms 1 and 2) (2) Fix second and fourth arms 2 and 4 (first and third arms) (Driven by the first and fourth arms 1 and 3) (3) Fix the second and third arms 3 and 3 (driven by the first and fourth arms 1 and 4) (4) First and fourth arms 1 and 4 (Drive by the second and third arms 2 and 3) (5) Fix the first and third arms 1 and 3 (Drive by the second and fourth arms 2 and 4) (6) First , Fix the second arms 1 and 2 (driven by the third and fourth arms 3 and 4)
【0015】さらに第4アーム4を姿勢角一定制御した
場合、次の3通りがある。 (7)第3のアーム3を固定 (第1,第2のアーム1,2で軌跡制御) (8)第2のアーム2を固定 (第1,第3のアーム1,3で軌跡制御) (9)第1のアーム1を固定 (第2,第3のアーム2,3で軌跡制御)Further, when the attitude of the fourth arm 4 is controlled to be constant, there are the following three types. (7) Fix third arm 3 (trajectory control with first and second arms 1 and 2) (8) Fix second arm 2 (trajectory control with first and third arms 1 and 3) (9) Fix first arm 1 (trajectory control with second and third arms 2 and 3)
【0016】以上の9通りの拘束条件を式で表すと次の
とおりである。 イ](1)の場合:T3V=0,T4V=0 ロ](2)の場合:T2V=0,T4V=0 ハ](3)の場合:T2V=0,T3V=0 ニ](4)の場合:T1V=0,T4V=0 ホ](5)の場合:T1V=0,T3V=0 ヘ](6)の場合:T1V=0,T2V=0 ト](7)の場合:T3V=0,T4V=T1V−T2V チ](8)の場合:T2V=0,T4V=T1V−T3V リ](9)の場合:T1V=0,T4V=−(T2V+T3V)The above nine conditions are expressed by the following equations. B) In the case of (1): T3V = 0, T4V = 0 b) In the case of (2): T2V = 0, T4V = 0 c) In the case of (3): T2V = 0, T3V = 0 d) (4) Case: T1V = 0, T4V = 0 e] (5): T1V = 0, T3V = 0 f) (6): T1V = 0, T2V = 0 to] (7): T3V = 0, T4V = T1V-T2Vh] (8): T2V = 0, T4V = T1V-T3V]] (9): T1V = 0, T4V =-(T2V + T3V)
【0017】ここで、(9),(10)式のX4V,Y4Vを速度指
令信号XV,YVとし、各拘束条件毎に上記条件を式
(9),(10)に代入して軌跡制御のために駆動する2本の
アームの角速度TnVを求めると、Here, X4V and Y4V in the equations (9) and (10) are used as velocity command signals XV and YV, and the above conditions are substituted into the equations (9) and (10) for each constraint condition, and the trajectory control is performed. The angular velocity TnV of the two arms driven for
【数7】 TnV=fn(XV,YV,T1,T2,T3,T4) …(11) となる。ただし、拘束条件(7)〜(9)では軌跡制御
対象部位は第3のアームの先端であるのでTnV = fn (XV, YV, T1, T2, T3, T4) (11) However, in the constraint conditions (7) to (9), the trajectory control target portion is the tip of the third arm,
【数8】 L4=0 …(12) として求める。その詳細な式を図3に示す。なお、図3
中、各種速度は「Xドット」あるいは「Yドット」のよ
うに示す。L4 = 0 (12) The detailed equation is shown in FIG. Note that FIG.
Medium speeds are indicated as “X dots” or “Y dots”.
【0018】次に制御装置を図1に基づいて説明する。
制御装置は,角度検出部100と、速度指令部200
と、拘束条件選択部300と、角速度制御値演算部40
0と、駆動制御値演算部500と、修正方向速度指令値
演算部600とが図のように接続されて構成されてい
る。Next, the control device will be described with reference to FIG.
The control device includes an angle detection unit 100 and a speed command unit 200
And the constraint condition selection unit 300 and the angular velocity control value calculation unit 40
0, a drive control value calculation unit 500, and a correction direction speed command value calculation unit 600 are connected as shown in the figure.
【0019】角度検出部100を構成する角度検出器1
01〜104は第1〜第4のアーム1〜4の回動支点に
取付けられ、周知のレバー機構とポテンショメ−タによ
りそれぞれのアームの相対角T1〜T4を検出し、各相対
角T1〜T4を拘束条件選択部300、角速度制御値演算
部400、駆動制御値演算部500および修正方向速度
指令値演算部600へ入力する。Angle detector 1 constituting angle detector 100
Reference numerals 01 to 104 are attached to the pivots of the first to fourth arms 1 to 4, and the relative angles T1 to T4 of the respective arms are detected by a well-known lever mechanism and a potentiometer. T4 is input to the constraint condition selector 300, the angular velocity control value calculator 400, the drive control value calculator 500, and the correction direction speed command value calculator 600.
【0020】速度指令部200は、軌跡制御方向の操作
レバー202で構成され、例えばレバー機構とポテンシ
ョメ−タによりレバー操作角度に相応した信号を出力
し、この信号を第4アーム4または第3アーム3の先端
の水平および垂直方向の速度指令値XV,YVとして角速
度制御値演算部400、符号器204および修正方向速
度指令値演算部600へも入力する。The speed command section 200 is constituted by an operating lever 202 in the trajectory control direction, and outputs a signal corresponding to the lever operating angle by a lever mechanism and a potentiometer, for example, and outputs this signal to the fourth arm 4 or the third arm. The speed command values XV and YV in the horizontal and vertical directions at the tip of the arm 3 are also input to the angular speed control value calculation unit 400, the encoder 204, and the correction direction speed command value calculation unit 600.
【0021】修正速度指令値演算部600は、図4に示
すように構成されており、座標演算部601で角度T1
〜T4から座標を演算し、軌跡制御対象部位のX座標を
出力する。座標記憶部602では、速度指令値YVから
軌跡制御が開始されたことを検知し、そのときのX座標
を記憶し、軌跡制御が終了するまでの値を保持する。そ
のときのX座標を軌跡制御対象開始点、換言すると 、
目標とする軌跡の位置X0とする。したがって、座標演
算部601から出力される実際の位置Xと座標記憶部6
02から出力される目標とする軌跡の位置X0との偏差
ΔXを加算点604で算出して修正速度演算部603に
入力する。修正速度演算部603では、次に示す式(1
3)から偏差ΔXと速度指令値YVから速度指令値XVと
を算出し、これを角速度制御値演算部400へ入力す
る。The modified speed command value calculating section 600 is configured as shown in FIG.
座標 T4 to calculate the coordinates and output the X coordinate of the locus control target portion. The coordinate storage unit 602 detects that the trajectory control has been started from the speed command value YV, stores the X coordinate at that time, and holds the value until the trajectory control ends. The X coordinate at that time is the starting point of the trajectory control target, in other words,
The position X0 of the target trajectory is set. Therefore, the actual position X output from the coordinate calculation unit 601 and the coordinate storage unit 6
The deviation ΔX from the target trajectory position X0 output from the target trajectory 02 is calculated at the addition point 604 and input to the correction speed calculation unit 603. The modified speed calculation unit 603 calculates the following equation (1
From 3), a speed command value XV is calculated from the deviation ΔX and the speed command value YV, and this is input to the angular speed control value calculation unit 400.
【数9】 XV=k・ΔX・|YV| …(13) ただし、kは定数XV = k · ΔX · | YV | (13) where k is a constant
【0022】拘束条件選択部300は、角度T1〜T4,
符号器204から出力される軌跡制御方向の速度指令値
の符号,およびモード設定器301の出力から、後述す
る処理手順によって拘束条件(1)〜(9)を選択出力
し、これを角速度制御値演算部400へ入力する。The constraint condition selector 300 determines the angles T1 to T4,
Based on the sign of the velocity command value in the trajectory control direction output from the encoder 204 and the output of the mode setting unit 301, the constraint conditions (1) to (9) are selectively output according to the processing procedure described later, and are output as angular velocity control values. It is input to the arithmetic unit 400.
【0023】ここで、モード設定器301は、オペレー
タの操作により、第4アーム4の相対角度が所定値にな
ると自動的に姿勢角一定制御を行うモードを選択するも
のである。その姿勢角一定制御モードの選択時、第4の
アーム4の姿勢角が所定角、例えば−90°になると、
上記9つの拘束条件の中から姿勢角一定制御を含む拘束
条件を優先して選択する。Here, the mode setting unit 301 selects a mode in which the attitude angle constant control is automatically performed when the relative angle of the fourth arm 4 reaches a predetermined value by the operation of the operator. When the posture angle constant control mode is selected, when the posture angle of the fourth arm 4 becomes a predetermined angle, for example, -90 °,
A constraint condition including constant attitude angle control is preferentially selected from the nine constraint conditions.
【0024】角速度制御値演算部400は、拘束条件に
よって演算ブロック401〜409の中から対応するい
ずれか1つの演算ブロックを選択し、角度T1〜T4およ
び速度指令値XV,YVから、拘束条件によって選択され
たアームの角速度制御値TnVを演算し、これらを駆動制
御値演算部500へ入力する。なお、各演算ブロック4
01〜409の出力は同一アームの制御値毎に各アーム
の加算点410〜422で加算され出力される。The angular velocity control value calculation section 400 selects any one of the calculation blocks from the calculation blocks 401 to 409 according to the constraint condition, and calculates the angle T1 to T4 and the speed command values XV, YV according to the constraint condition. The angular velocity control values TnV of the selected arm are calculated, and these are input to the drive control value calculation unit 500. Note that each operation block 4
The outputs 01 to 409 are added and output at the addition points 410 to 422 of each arm for each control value of the same arm.
【0025】駆動制御値演算部500は、次の式に基づ
いて角速度制御値TnVおよび角度T1〜T4から、シリン
ダ5〜8の流量制御値Q1〜Q4を演算し、それを電気油
圧変換弁11〜14に入力する。The drive control value calculation unit 500 calculates the flow control values Q1 to Q4 of the cylinders 5 to 8 from the angular velocity control value TnV and the angles T1 to T4 based on the following equations, To 14.
【数10】 Qn=TnV・gn(Tn)・An …(14) ここで、gn(Tn)はリンク補正係数 Anはシリンダ受圧面積Qn = TnV ・ gn (Tn) ・ An (14) where gn (Tn) is a link correction coefficient An is a cylinder pressure receiving area
【0026】これら電気油圧変換弁11〜14には油圧
源から圧油が導かれており、入力される流量制御値Q1
〜Q4に応じた流量および方向で圧油を第1〜第4アー
ム1〜4用のシリンダ5〜8に供給し、軌跡制御が行わ
れる。A pressure oil is guided to these electro-hydraulic conversion valves 11 to 14 from a hydraulic pressure source.
Pressure oil is supplied to the cylinders 5 to 8 for the first to fourth arms 1 to 4 at a flow rate and a direction corresponding to Q4 to perform trajectory control.
【0027】拘束条件選択部300において、制御すべ
きアームの選択は図5に示す処理手順により行われる。
ステップS20においてモード設定器301の出力を読
み込み、第4アーム4の姿勢角一定制御モードか否かを
判定する。姿勢角一定制御モードが選択されていなけれ
ばステップS2へ、選択されているとステップS1へ進
む。ステップS1では、第4アーム4の相対角度が予め
定めた所定角か否かを判定し、所定角に達していなけれ
ばステップS2に進み、所定角に達していればステップ
S3へ進む。ステップS2では、図2(a)に示す各ア
ームの回動支点と第4アーム4先端を結ぶ線分がX軸と
なす角(制御角と呼ぶ)A14,A24,A34,A44を計算
する。In the constraint condition selecting section 300, the selection of the arm to be controlled is performed according to the processing procedure shown in FIG.
In step S20, the output of the mode setting unit 301 is read, and it is determined whether or not the fourth arm 4 is in the constant attitude angle control mode. If the constant attitude angle control mode has not been selected, the process proceeds to step S2, and if it has been selected, the process proceeds to step S1. In step S1, it is determined whether or not the relative angle of the fourth arm 4 is a predetermined angle. If the angle has not reached the predetermined angle, the process proceeds to step S2. If the angle has reached the predetermined angle, the process proceeds to step S3. In step S2, angles (called control angles) A14, A24, A34, and A44 formed by the line segment connecting the rotation fulcrum of each arm and the tip of the fourth arm 4 shown in FIG.
【0028】前述の各アーム先端の座標を用いると、Using the coordinates of the tip of each arm described above,
【数11】 A14=|tan-1(Y4/X4)| …(15)A14 = | tan −1 (Y4 / X4) | (15)
【数12】 A24=|tan-1{(Y4−Y1)/(X4−X1)}| …(16)A24 = | tan -1 {(Y4-Y1) / (X4-X1)} | (16)
【数13】 A34=|tan-1{(Y4−Y2)/(X4−X2)}| …(17)A34 = | tan -1 {(Y4-Y2) / (X4-X2)} | (17)
【数14】 A44=|tan-1{(Y4−Y3)/(X4−X3)}| …(18) と表すことができる。A44 = | tan -1 {(Y4-Y3) / (X4-X3)} | (18)
【0029】同様にステップS3では図2(b)に示す
各アームの回動支点と第3アーム3先端を結ぶ線分がX
軸となす各(制御角)A13,A23,A33を計算する。Similarly, in step S3, a line connecting the pivot point of each arm and the tip of the third arm 3 shown in FIG.
The respective (control angles) A13, A23, and A33 that form the axis are calculated.
【数15】 A13=|tan-1(Y3/X3)| …(19)A13 = | tan −1 (Y3 / X3) | (19)
【数16】 A23=|tan-1{(Y3−Y2)/(X3−X2)}| …(20)A23 = | tan -1 {(Y3-Y2) / (X3-X2)} | (20)
【数17】 A33=|tan-1{(Y3−Y2)/(X3−X2)}| …(21)A33 = | tan -1 {(Y3-Y2) / (X3-X2)} | (21)
【0030】次にステップS4では、各アームの制御角
を比べ、最も0°に近い制御角のアームを選択する。こ
うして選択されたアームは、その回動によって生じる軌
跡制御対象部位の移動量がX軸方向には最小となりY軸
方向には最大となるアームであり、言い換えればY軸方
向の制御に最も適したアームである。ステップS5で
は、ステップS4で選択したアームが制御方向でストロ
ークエンドとなっていないかを判定する。この判定は図
6に示すように、たとえば第1アームの場合、速度指令
値YVが負のときは当該アームの相対角が予め定めた最
小値(min)より小さいか否か、あるいはYVが正のとき
は予め定めた最大値(max)より大きいか否かを判定し
て行われる。Next, in step S4, the control angles of the respective arms are compared, and the arm having the control angle closest to 0 ° is selected. The arm selected in this manner is an arm in which the amount of movement of the trajectory control target portion caused by its rotation is the smallest in the X-axis direction and the largest in the Y-axis direction, in other words, the arm most suitable for control in the Y-axis direction. Arm. In step S5, it is determined whether the arm selected in step S4 has reached the stroke end in the control direction. As shown in FIG. 6, for example, in the case of the first arm, when the speed command value YV is negative, it is determined whether the relative angle of the arm is smaller than a predetermined minimum value (min), or YV is positive. In this case, it is determined whether the value is larger than a predetermined maximum value (max).
【0031】ストロークエンドの場合はステップS6に
進み、未選択のアーム数が1本か否かを判定する。1本
の場合はアームを2本選択することができないのでステ
ップS13へ進み制御不可とする。すなわち拘束条件を
出力しない。未選択のアーム数が2本以上の場合はステ
ップS7へ進み、ステップS5でストロークエンドとな
ったアームを除いた中から制御角が最も0°に近いアー
ムを選択し、同様にストロークエンドを判定するためス
テップS5へ戻る。In the case of the stroke end, the process proceeds to step S6, and it is determined whether or not the number of unselected arms is one. In the case of one arm, it is impossible to select two arms, so the process proceeds to step S13, and control is disabled. That is, no constraint condition is output. If the number of unselected arms is two or more, the process proceeds to step S7, and the arm whose control angle is closest to 0 ° is selected from among the arms excluding the arm at the stroke end in step S5, and the stroke end is similarly determined. Then, the process returns to step S5.
【0032】ステップS5でストロークエンドでない場
合はステップS8へ進み、各アームの制御角を比べ、最
も90°に近い制御角のアームを選択する。ここで選択
されたアームは、その回動によって生じる軌跡制御対象
部位の移動量がX軸方向に最大となりY軸方向には最小
となるアームであり、言い換えればX軸方向の制御に最
も適したアームである。If it is determined in step S5 that the stroke is not at the end of the stroke, the flow advances to step S8 to compare the control angles of the respective arms, and select an arm having a control angle closest to 90 °. The arm selected here is an arm in which the amount of movement of the trajectory control target portion caused by its rotation is maximum in the X-axis direction and minimum in the Y-axis direction, in other words, the arm most suitable for control in the X-axis direction. Arm.
【0033】ステップS9では、ステップS8で選択し
たアームが時計回り方向および反時計回り方向のいずれ
か一方がストロークエンドになっているかを判定する。
いずれの回動方向ともストロークエンドでない場合は、
ステップS4で選択したアームと合わせてアームが2本
選択されたことになり、ステップS12へ進んで選択し
たアームに対応する拘束条件(1)〜(9)のいずれか
を出力する。In step S9, it is determined whether the arm selected in step S8 has reached the stroke end in either the clockwise direction or the counterclockwise direction.
If neither of the rotation directions is at the stroke end,
This means that two arms have been selected together with the arm selected in step S4, and the flow advances to step S12 to output one of the constraint conditions (1) to (9) corresponding to the selected arm.
【0034】ステップS9でいずれか一方の回動方向が
ストロークエンドと判定された場合はステップS10で
未選択のアーム数を調べ、残っていない場合はアームを
2本選択することができないのでステップS13へ進み
制御不可とする。未選択のアームが残っている場合はス
テップS11へ進み、ステップS9でストロークエンド
となったアームを除いた中から制御角が最も90°に近
いアームを選択し、同様にストロークエンドを判定する
ためステップS9へ戻る。If it is determined in step S9 that one of the turning directions is the stroke end, the number of unselected arms is checked in step S10, and if there is no remaining arm, two arms cannot be selected, so step S13 Proceed to and control is disabled. If an unselected arm remains, the process proceeds to step S11, and an arm having a control angle closest to 90 ° is selected from among the arms excluding the arm at stroke end in step S9, and the stroke end is similarly determined. It returns to step S9.
【0035】この手順は、垂直軌跡制御を想定したもの
であるが、水平軌跡制御では、ステップS4,ステップ
S7とステップS8,11を入れ替えればよい。This procedure assumes vertical trajectory control. In horizontal trajectory control, steps S4 and S7 and steps S8 and S11 may be interchanged.
【0036】次に本装置の動作について説明する。図示
しない電源スイッチを投入するとこの装置が起動し、角
度検出器101〜104で検出された角度T1〜T4,軌
跡制御レバー202で指令された速度指令値YVの符号
およびモード設定器301で設定されたモードに基づい
て、拘束条件選択部300は、図5の手順により軌跡制
御に最も適した2本のアームを選択し、選択されたアー
ムに対応する拘束条件を出力する。修正方向速度指令値
演算部600では、速度指令値YVと角度T1〜T4から
偏差修正を加えた修正方向速度指令値XVを演算して出
力する。Next, the operation of the present apparatus will be described. When the power switch (not shown) is turned on, the apparatus starts up, and the angles T1 to T4 detected by the angle detectors 101 to 104, the sign of the speed command value YV commanded by the trajectory control lever 202, and the setting by the mode setting device 301 are set. Based on the selected mode, the constraint condition selection unit 300 selects the two arms most suitable for the trajectory control according to the procedure of FIG. 5 and outputs the constraint condition corresponding to the selected arm. The corrected direction speed command value calculation unit 600 calculates and outputs a corrected direction speed command value XV obtained by adding a deviation correction from the speed command value YV and the angles T1 to T4.
【0037】角速度制御値演算部400では、選択され
た拘束条件に対応するいずれかの演算ブロックが選択さ
れ、角度T1〜T4および速度指令値XV,YVから、選択
されたアームの角速度制御値TnVが演算される。選択さ
れていない演算ブロックの角速度制御値TnVは零なの
で、各演算ブロック出力のうち同一アームごとにそのア
ーム角速度制御値を加算点410〜420で加算すれ
ば、選択された一対のアームの角速度制御値の切換えが
行われる。In the angular velocity control value calculation section 400, one of the arithmetic blocks corresponding to the selected constraint condition is selected, and the angular velocity control value TnV of the selected arm is determined from the angles T1 to T4 and the velocity command values XV, YV. Is calculated. Since the angular velocity control values TnV of the non-selected arithmetic blocks are zero, the angular velocity control values of the selected pair of arms can be obtained by adding the arm angular velocity control values of the outputs of the arithmetic blocks for the same arm at the addition points 410 to 420. Switching of the value is performed.
【0038】これらの角速度制御値T1V〜T4Vは駆動制
御値演算部500でリンク補正され、各アームの流量制
御値Q1〜Q4に変換される。これらの流量制御値Q1〜
Q4は電気油圧変換弁11〜14に供給され、油圧源か
らの圧油が所定方向、所定流量にて第1〜第4シリンダ
5〜8に供給される。これにより選択されたアームが回
動して第4アーム4先端の軌跡が速度指令レバーの操作
に応じて制御される。あるいは、第3アーム3先端の軌
跡が速度指令レバーの操作に応じて制御され、第4アー
ム4の姿勢角が一定に制御される。These angular velocity control values T1V to T4V are link-corrected by the drive control value calculation section 500 and converted into flow control values Q1 to Q4 for each arm. These flow control values Q1
Q4 is supplied to the electro-hydraulic conversion valves 11 to 14, and pressure oil from a hydraulic source is supplied to the first to fourth cylinders 5 to 8 at a predetermined flow rate in a predetermined direction. As a result, the selected arm rotates and the trajectory of the tip of the fourth arm 4 is controlled in accordance with the operation of the speed command lever. Alternatively, the trajectory of the tip of the third arm 3 is controlled according to the operation of the speed command lever, and the attitude angle of the fourth arm 4 is controlled to be constant.
【0039】このようにストロークエンド判定方法をス
テップS9のように制御方向に関してストロークになっ
ていないものを選択する従来方法から、制御方向にかか
わらず両回動方向のいずれもストロークエンドになって
いないものを選択する方法に変えることにより、図11
(b)で示す第1のアーム1のように、偏差がでていな
いときは時計回り方向に回動し偏差によっては反時計回
り方向に回動するようなアームの場合でも、両回動方向
がともにストロークエンドでないアームが選択されるの
で、偏差フィードバック制御を適用する場合においても
制御不能となることがない。すなわち、従来方式では、
修正側アームとして制御方向がストロークエンドではな
いとして選択されたアームでも、修正のための回動方向
にはストロークエンドである場合があり、その場合には
制御が不能となることがあったが、本実施例によれば、
修正のための回動方向がストロークエンドにあるアーム
は事前に除かれるから、そのような制御不能に陥ること
がない。また、アームの姿勢を参照するだけでストロー
クエンドの判定ができるので演算時間が短縮できる。As described above, the stroke end judging method is different from the conventional method in which the stroke is not determined in the control direction as in step S9. By changing to the method of selecting things, FIG.
As in the case of the first arm 1 shown in (b), even in the case of an arm that rotates clockwise when there is no deviation and rotates counterclockwise depending on the deviation, both arms rotate in both directions. Are not stroke ends, the control is not disabled even when the deviation feedback control is applied. That is, in the conventional method,
Even if the control direction is not the stroke end as the correction side arm, the rotation direction for correction may be the stroke end in some cases, and in that case, control may not be possible, According to the present embodiment,
Since the arm whose rotation direction for correction is at the stroke end is removed in advance, such an uncontrollable state does not occur. Further, since the stroke end can be determined only by referring to the posture of the arm, the calculation time can be reduced.
【0040】−第2の実施例− 次に図8および図9により本発明による多関節作業機の
軌跡制御装置の他の実施例を説明する。第1の実施例で
は時計回り方向および反時計回り方向の両回動方向がと
もにストロークエンドではないアームが選択されないよ
うにしたが、第2の実施例では、上述の偏差フィードバ
ック制御をせずに軌跡制御する場合のアーム回動方向
と、偏差フィードバック制御を行って軌跡制御する場合
のアーム回動方向とが同一か否かを判定し、同一であれ
ば前者の方向がストロークエンドであれば後者の方向が
ストロークエンドか否かを調べるまでもなく、そのアー
ムを選択しないようにしたものである。Second Embodiment Next, another embodiment of a trajectory control device for an articulated working machine according to the present invention will be described with reference to FIGS. In the first embodiment, the arm in which both the clockwise direction and the counterclockwise direction are not at the stroke end is not selected. However, in the second embodiment, the above-described deviation feedback control is performed without performing the above-described deviation feedback control. It is determined whether or not the arm rotation direction when performing trajectory control and the arm rotation direction when performing trajectory control by performing deviation feedback control are the same. If the same, the former is the stroke end and the latter is the stroke end. The arm is not selected without checking whether the direction is the stroke end or not.
【0041】図8と図9において、図1および図5と同
一の箇所には同一の符号を付して相違点を主に説明す
る。修正方向速度指令値演算部600で演算された修正
方向速度指令値XVは拘束条件選択部300Aに入力さ
れる。拘束条件選択部300Aは、図9のフローチャー
トにより制御すべきアームを選択する。8 and 9, the same parts as those in FIGS. 1 and 5 are denoted by the same reference numerals, and the differences will be mainly described. The correction direction speed command value XV calculated by the correction direction speed command value calculation unit 600 is input to the constraint condition selection unit 300A. The constraint condition selection unit 300A selects an arm to be controlled according to the flowchart of FIG.
【0042】図9のフローチャートにおいて、ステップ
S8で最も90°に近い制御角のアームを選択した後、
ステップS21で修正方向速度指令値XVを読み込む。
次いでステップS22において、修正方向と制御方向が
同一が否かを判定し、異なっていればステップS10に
進み、同一であればステップS23に進む。ここで、ス
テップS22では、偏差フィードバックを行なわずに軌
跡制御する場合のアーム制御回動方向と、読み込まれた
修正方向速度指令値XVからわかる修正回動方向とが一
致しているかを判定する。ステップS23では制御回動
方向がストロークエンドか否かを判定し、ストロークエ
ンドであればステップS10に進み、ストロークエンド
でなければステップS12に進む。In the flowchart of FIG. 9, after selecting the arm having the control angle closest to 90 ° in step S8,
In step S21, the corrected direction speed command value XV is read.
Next, in step S22, it is determined whether or not the correction direction and the control direction are the same. If they are different, the process proceeds to step S10. Here, in step S22, it is determined whether or not the arm control rotation direction in the case of performing the trajectory control without performing the deviation feedback coincides with the correction rotation direction known from the read correction direction speed command value XV. In step S23, it is determined whether or not the control rotation direction is the stroke end. If it is the stroke end, the process proceeds to step S10, and if not, the process proceeds to step S12.
【0043】ステップS10では、未選択のアーム数を
調べ、残っていない場合にはアームを2本選択できない
のでステップS13へ進み、制御不可とする。未選択の
アームが残っていれば、ステップS11に進み、ステッ
プS22で同一方向でなかったアームとステップS23
でストロークエンドとなったアームを除いた中から制御
角が最も90°に近いアームを選択しステップS22に
戻る。In step S10, the number of unselected arms is checked. If there is no remaining arm, two arms cannot be selected, so the flow advances to step S13 to disable control. If an unselected arm remains, the process proceeds to step S11, where the arm not in the same direction in step S22 and step S23
Then, an arm whose control angle is closest to 90 ° is selected from among the arms excluding the arm at the stroke end, and the process returns to step S22.
【0044】先に説明した第1の実施例では、いずれか
一方のアーム回動方向がストロークエンドであるアーム
は選択されなかった。換言すると両回動方向ともにスト
ロークエンドでないアームだけが選択された。しかし、
第2の実施例では、制御回動方向と修正回動方向が同一
である場合には、その回動方向さえストロークエンドで
なければそのアームが選択されるから、アームを有効に
利用できる。In the first embodiment described above, the arm whose one of the arm rotation directions is the stroke end is not selected. In other words, only the arms that were not stroke end in both directions of rotation were selected. But,
In the second embodiment, when the control rotation direction and the correction rotation direction are the same, the arm is selected if the rotation direction is not the stroke end, so that the arm can be used effectively.
【0045】なお、拘束条件選択部300をソフトウエ
アの形態で構成したが、予め角度T1〜T4,速度指令値
XV,YVの符号およびモード設定信号の種々の組合せに
対する拘束条件を計算しておき、それらの組合せに対す
る拘束条件をテーブル化してもよい。この場合、入力デ
ータでテーブルを参照すれば直ちに拘束条件が求まり図
5,9の処理時間の短縮が可能である。また、拘束条件
に対応する演算ブロックを選択して角速度制御値を演算
すれば良いので、すべてのアームの組合せに対して角速
度制御値を演算する必要がなく、処理時間はアーム数が
増えても変わらない。さらに、モード設定器301を設
けることにより、容易に姿勢角一定制御への切換えを行
うことができる。Although the constraint condition selecting section 300 is configured in the form of software, constraint conditions for various combinations of angles T1 to T4, signs of speed command values XV and YV, and mode setting signals are calculated in advance. , The constraint conditions for these combinations may be tabulated. In this case, the constraint condition is immediately obtained by referring to the table with the input data, and the processing time in FIGS. 5 and 9 can be reduced. Further, since it is only necessary to select the calculation block corresponding to the constraint condition and calculate the angular velocity control value, it is not necessary to calculate the angular velocity control value for all arm combinations, and the processing time is longer even if the number of arms increases. does not change. Further, by providing the mode setting device 301, it is possible to easily switch to the attitude angle constant control.
【0046】また、各演算ブロックの角速度制御値を同
一アーム毎に加算しているので、拘束条件の切換えが自
動的に行われる。なお、この切換に際して、旧データを
漸減し新データを漸増して切換時のショックを緩和する
こともできる。さらにまた、姿勢角一定制御モードを選
択すれば、第4のアーム4が予め定めた所定値、例えば
−90°になると自動的に姿勢角一定制御が行われるの
で、小径の垂直孔などへ作業用アタッチメントを移動さ
せる際などに孔と先端アームとの接触を防止できる。Further, since the angular velocity control values of the respective operation blocks are added for each arm, the constraint condition is automatically switched. At the time of this switching, the old data can be gradually reduced and the new data can be gradually increased to alleviate the shock at the time of switching. Furthermore, if the attitude control mode is selected, when the fourth arm 4 reaches a predetermined value, for example, -90 °, the attitude angle control is automatically performed. When the attachment is moved, contact between the hole and the tip arm can be prevented.
【0047】なお、本発明を適用するにあたっては以上
の実施例の各構成要素を次のようにしても良い。 アーム数は4本に限定されない。 各アームを油圧シリンダで駆動したが、油圧に限定さ
れず、また油圧モータ、油圧ロータリアクチュエータな
どその他のアクチュエータを用いることができる。 バイブロハンマ,アースオーガや掘削バケット等に使
用できる旨述べたが、その他の各種作業用アタッチメン
トにも使用できる。 第1アーム1の角度を上部旋回体に対する相対角で検
出したが、第1アームの対地角を検出しても良く、ま
た、作業機本体の傾斜角を検出して相対角を補正しても
良い。 角度検出器として、磁気抵抗素子を用いたもの、差動
コイルを用いたもの、光学式、磁気式のロータリエンコ
ーダを用いたものなどポテンショメ−タに限定されな
い。In applying the present invention, each component of the above embodiment may be configured as follows. The number of arms is not limited to four. Although each arm is driven by a hydraulic cylinder, it is not limited to hydraulic pressure, and other actuators such as a hydraulic motor and a hydraulic rotary actuator can be used. Although it has been described that it can be used for a vibro hammer, earth auger, excavating bucket, etc., it can also be used for various other work attachments. Although the angle of the first arm 1 is detected as a relative angle with respect to the upper swing body, the angle of the first arm with respect to the ground may be detected, or the inclination angle of the work implement body may be detected to correct the relative angle. good. The angle detector is not limited to a potentiometer, such as one using a magnetoresistive element, one using a differential coil, one using an optical or magnetic rotary encoder.
【0048】[0048]
【発明の効果】本発明によれば次のような効果が得られ
る。すなわち、一対の駆動アームを選択して軌跡制御対
象部位を軌跡制御駆動する際、軌跡制御対象部位の目標
軌跡からのずれ量をアームの角速度指令値にフィードバ
ック制御する軌跡制御装置において、一対のアームを選
択する際に、制御方向がストロークエンドであるアーム
を排除する従来方式では、制御方向はストロークエンド
でなく選択されたアームでも修正方向にストロークエン
ドである場合には、偏差フィードバックができず、制御
不能状態に陥ってしまったが、本発明のように、軌跡制
御のために駆動される一対のアームを選択する際に、ア
ームの両回動方向ともストロークエンドでないアームを
選択するようにしたので、偏差フィードバック制御中に
制御不能に陥らなくなる。したがって、上記偏差フィー
ドバック制御方式で軌跡制御する場合でも、作業機のア
ーム数や姿勢によらず軌跡制御に最も適した2本のアー
ムの組を自動的に選択することが可能となる。また、ず
れ量によるフィードバック制御を行なわない場合のアー
ム回動方向とずれ量によるフィードバック制御を行なう
場合のアーム回動方向とが同一か否かを判定し、同一で
あれば制御方向のみストロークエンドであることを判定
するようにしてアームを選択する方式でも上記と同様な
作用効果が得られるとともに、アームを有効に利用でき
る。According to the present invention, the following effects can be obtained. In other words, when a pair of drive arms are selected to perform trajectory control driving of a trajectory control target site, a trajectory control device that feedback-controls a deviation amount of the trajectory control target site from a target trajectory to an angular velocity command value of the arm, In the conventional method of excluding the arm whose control direction is the stroke end when selecting, when the control direction is not the stroke end but the selected arm is also the stroke end in the correction direction, deviation feedback cannot be performed, Although the robot has fallen into an uncontrollable state, as in the present invention, when selecting a pair of arms driven for trajectory control, an arm that does not have a stroke end in both rotation directions of the arm is selected. Therefore, control is not lost during the deviation feedback control. Therefore, even in the case of performing the trajectory control using the above-described deviation feedback control method, it is possible to automatically select a set of two arms most suitable for the trajectory control regardless of the number of arms and the posture of the work implement. Also, it is determined whether the arm rotation direction when not performing the feedback control based on the deviation amount is the same as the arm rotation direction when performing the feedback control based on the deviation amount, and if the same, only the control direction is performed at the stroke end. The same effect as described above can be obtained with the method of selecting an arm by judging that there is, and the arm can be used effectively.
【図1】本発明の一実施例の構成図FIG. 1 is a configuration diagram of an embodiment of the present invention.
【図2】座標を定義する図FIG. 2 is a diagram for defining coordinates.
【図3】拘束条件の詳細を示す図FIG. 3 is a diagram showing details of constraint conditions.
【図4】修正方向速度指令値演算手段の詳細ブロック図FIG. 4 is a detailed block diagram of a correction direction speed command value calculating means.
【図5】拘束条件選択部の処理手順を示す図FIG. 5 is a diagram showing a processing procedure of a constraint condition selecting unit.
【図6】ストロークエンドの判定条件を説明する図FIG. 6 is a view for explaining conditions for determining a stroke end.
【図7】本発明が適用される作業機の一実施例の側面図FIG. 7 is a side view of an embodiment of a working machine to which the present invention is applied.
【図8】本発明の他の実施例の構成図FIG. 8 is a configuration diagram of another embodiment of the present invention.
【図9】拘束条件選択部の処理手順を示す図FIG. 9 is a diagram showing a processing procedure of a constraint condition selecting unit.
【図10】本発明が適用される作業機の他の例の側面図FIG. 10 is a side view of another example of a working machine to which the present invention is applied.
【図11】従来の問題点を説明する図FIG. 11 is a diagram illustrating a conventional problem.
1〜4:アーム 5〜8:油圧シリンダ 11〜14:電気油圧変換弁 100:角度検出手段 101〜104:角度検出器 200:速度指令部 202:速度指令レバー 300,300A:拘束条件選択部 301:モード設定器 400:角速度制御値演算部 401〜409:演算ブロック 500:駆動制御値演算部 600:修正方向速度指令値演算手段 1-4: Arm 5-8: Hydraulic cylinder 11-14: Electro-hydraulic conversion valve 100: Angle detecting means 101-104: Angle detector 200: Speed command unit 202: Speed command lever 300, 300A: Constraint condition selecting unit 301: Mode setting unit 400: angular velocity control value calculation unit 401 to 409: calculation block 500: drive control value calculation unit 600: correction direction speed command value calculation means
───────────────────────────────────────────────────── フロントページの続き (58)調査した分野(Int.Cl.7,DB名) G05B 19/18 - 19/46 E02F 3/43,9/20 ──────────────────────────────────────────────────続 き Continued on the front page (58) Field surveyed (Int.Cl. 7 , DB name) G05B 19/18-19/46 E02F 3 / 43,9 / 20
Claims (2)
以上のアームと、 それらのアームを駆動するアクチュエータと、 各アームの姿勢を検出する姿勢検出手段と、 軌跡対象部位の目標軌跡からのずれ量を演算するずれ量
演算手段と、 軌跡制御対象部位の目標軌跡に沿う作業方向の作業速度
を指令する速度指令手段と、 前記ずれ量と前記作業速度とに基づいて目標軌跡に直交
する修正方向の修正方向速度指令値を演算する修正方向
速度指令値演算手段と、 前記作業方向の移動量が最大となり、前記修正方向には
最小となるアームであって制御方向にストロークエンド
ではないアームを選択するとともに、修正方向には最大
となり作業方向には最小となるアームであって、いずれ
かの回動方向がストロークエンドではないアームを選択
し、選択した2本のアームに応じた拘束条件を出力する
拘束条件選択手段と、 前記拘束条件と検出されたアーム姿勢と作業速度指令値
と修正方向速度指令値とに基づいて、前記軌跡制御対象
部位が前記目標軌跡上を移動するように、選択されたア
ームの回動角速度を演算するアーム角速度制御値演算手
段と、 演算されたアーム角速度でアームが回動するように前記
アクチュエータを駆動する駆動制御値演算手段とを具備
すること特徴とする多関節作業機の軌跡制御装置。1. An arm which drives three or more arms rotatably connected via a joint, an actuator for driving the arms, a posture detecting means for detecting a posture of each arm, and a target trajectory of a trajectory target portion A shift amount calculating means for calculating a shift amount from the position, a speed commanding means for commanding a work speed in a work direction along a target locus of the locus control target portion, and a target locus based on the shift amount and the work speed. A correction direction speed command value calculating means for calculating a correction direction speed command value of the correction direction to be performed; and an arm having a maximum movement amount in the working direction and a minimum in the correction direction and not a stroke end in the control direction. In addition to selecting the arm, select the arm that is the largest in the correction direction and the smallest in the working direction, and one of the rotation directions is not at the stroke end. A constraint condition selecting means for outputting a constraint condition according to the selected two arms; and a trajectory control target portion based on the constraint condition, the detected arm posture, a work speed command value, and a correction direction speed command value. Means for calculating a rotation angular velocity of the selected arm so that the arm moves on the target trajectory; and drive control for driving the actuator so that the arm rotates at the calculated arm angular velocity. A trajectory control device for an articulated working machine, comprising: a value calculating means.
以上のアームと、 それらのアームを駆動するアクチュエータと、 各アームの姿勢を検出する姿勢検出手段と、 軌跡対象部位の目標軌跡からのずれ量を演算するずれ量
演算手段と、 軌跡制御対象部位の目標軌跡に沿う作業方向の作業速度
を指令する速度指令手段と、 前記ずれ量と前記作業速度とに基づいて目標軌跡に直交
する修正方向の修正方向速度指令値を演算する修正方向
速度指令値演算手段と、 前記作業方向の移動量が最大となり、前記修正方向には
最小となるアームを選択するとともに、修正方向には最
大となり作業方向には最小となるアームであって、前記
ずれ量によるフィードバック制御を行なわない場合のア
ーム回動方向と前記フィードバック制御を行う場合のア
ーム回動方向とが同一である場合にはその方向でストロ
ークエンドでないアームを選択し、選択した2本のアー
ムに応じた拘束条件を出力する拘束条件選択手段と、 前記拘束条件と前記アーム姿勢と前記作業速度指令値と
修正方向速度指令値とに基づいて、前記軌跡制御対象部
位が前記目標軌跡上を移動するように、選択されたアー
ムの回動角速度を演算するアーム角速度制御値演算手段
と、 演算されたアーム角速度でアームが回動するように前記
アクチュエータを駆動する駆動制御値演算手段とを具備
すること特徴とする多関節作業機の軌跡制御装置。2. An arm for driving three or more arms rotatably connected via a joint, an actuator for driving the arms, a posture detecting means for detecting a posture of each arm, and a target trajectory of a trajectory target portion A shift amount calculating means for calculating a shift amount from the position, a speed commanding means for commanding a work speed in a work direction along a target locus of the locus control target portion, and a target locus based on the shift amount and the work speed. A correction direction speed command value calculating means for calculating a correction direction speed command value of the correction direction to be selected; and an arm in which the movement amount in the working direction is maximum and the arm in the correction direction is minimum, The arm rotation direction when the feedback control based on the deviation amount is not performed, and the arm rotation direction when the feedback control is performed based on the displacement amount. When the moving direction is the same, an arm that is not a stroke end in that direction is selected, and a constraint condition selecting unit that outputs a constraint condition according to the selected two arms; Arm angular velocity control value computing means for computing a rotational angular velocity of the selected arm, based on the work velocity instruction value and the correction direction velocity instruction value, so that the trajectory control target moves on the target trajectory, A trajectory control device for an articulated working machine, comprising: drive control value calculation means for driving the actuator so that the arm rotates at the calculated arm angular velocity.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP33688093A JP3338540B2 (en) | 1993-12-28 | 1993-12-28 | Trajectory control device for articulated work machines |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP33688093A JP3338540B2 (en) | 1993-12-28 | 1993-12-28 | Trajectory control device for articulated work machines |
Publications (2)
Publication Number | Publication Date |
---|---|
JPH07200015A JPH07200015A (en) | 1995-08-04 |
JP3338540B2 true JP3338540B2 (en) | 2002-10-28 |
Family
ID=18303513
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP33688093A Expired - Fee Related JP3338540B2 (en) | 1993-12-28 | 1993-12-28 | Trajectory control device for articulated work machines |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP3338540B2 (en) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2015068071A (en) * | 2013-09-30 | 2015-04-13 | 日立建機株式会社 | Construction machine |
-
1993
- 1993-12-28 JP JP33688093A patent/JP3338540B2/en not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
JPH07200015A (en) | 1995-08-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP3056254B2 (en) | Excavation control device for construction machinery | |
JP3091667B2 (en) | Excavation control device for construction machinery | |
US5799419A (en) | Method for controlling the operation of power excavator | |
JP3364303B2 (en) | Work machine control device | |
JPH093959A (en) | Control of hydraulically operated construction equipment with plurality of tandem connecting member | |
CN111794298B (en) | System for controlling movement of multiple links of an excavator | |
JP3145027B2 (en) | Automatic control device for hydraulic excavator | |
JP3441886B2 (en) | Automatic trajectory control device for hydraulic construction machinery | |
JP3338540B2 (en) | Trajectory control device for articulated work machines | |
JP2677812B2 (en) | Control equipment for hydraulic machines | |
JP2000303492A (en) | Front controller for construction machinery | |
JPH0414531A (en) | Controller for operating machine | |
JP3682352B2 (en) | Front control device for construction machinery | |
JPH02232430A (en) | Controller for depth of excavation of hydraulic shovel | |
JPH0776453B2 (en) | Work machine trajectory control device | |
JP2000355957A (en) | Zone restrictive excavation controller for hydraulic shovel | |
JP3705661B2 (en) | Attitude control device for construction machine work equipment | |
JP2721325B2 (en) | Method of operating an articulated erector device and its device | |
JPH02287804A (en) | Locus controller for articulated work machine | |
JP2672724B2 (en) | Trajectory control device with articulated arm | |
JPH10317417A (en) | Posture-controller for multi-joint working machine | |
WO2023153202A1 (en) | Work area setting system | |
JPH10292420A (en) | Excavating locus control device of hydraulic shovel | |
JP3681527B2 (en) | Operation control device for 2-piece boom type work machine | |
JPS62164921A (en) | Controller for master lever type power shovel |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
LAPS | Cancellation because of no payment of annual fees |