JP3338540B2 - Trajectory control device for articulated work machines - Google Patents

Description

【発明の詳細な説明】DETAILED DESCRIPTION OF THE INVENTION

【０００１】[0001]

【産業上の利用分野】本発明は、３本以上のアームを有
する多関節作業機の軌跡制御装置に関する。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]

【従来の技術】３本以上のアームを有する多関節作業機
で軌跡制御を行う場合、駆動アームを２本選択し、残り
のアームは固定する方式がとられる。駆動すべき２本の
アームを選択する方式として特開平２−２８７８０４号
公報に開示されている方法が知られている。この方法
は、駆動アームとして、アームを回動させたときに軌跡
制御対象部位がＸ（水平）方向に最大となりＹ（垂直）
方向に最小となるアームと、Ｙ方向に最大となりＸ方向
に最小となるアームを選択する。そして、それぞれのア
ームについて制御方向に対してストロークエンドか否か
の判定を行なっている。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.

【０００３】たとえば、垂直下げの軌跡制御を行なう場
合、図１１（ａ）に示すような姿勢のときは第３アーム
４がＹ軸方向の制御に最も適したアームとして、第１ア
ーム１がＸ軸方向の制御に最も適したアームとして選択
される。アームの回動方向は作業方向がＹ軸方向となっ
ているので第３アーム３が時計回りに回動する。第１ア
ーム１は第３アーム３の回動により生じる軌跡制御位置
からのずれを補正する動きをするため、角度Ａ３４に応
じて時計回りあるいは反時計回り方向に回動する。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.

【０００４】さらに、直線軌跡制御を行なう場合とし
て、特開平１−２７８６２３号公報に開示されている方
法が知られている。この従来技術では、Ｙ方向を作業方
向にＸ方向を修正方向にしている。アーム先端を作業方
向に軌跡制御するときの修正方向のずれ量（偏差Δｘ）
を検出し、そのずれ量と速度指令レバーにより与えられ
る作業方向の速度指令値ＹVとから偏差修正方向の速度
指令値ＸVを求める。そして、アーム先端が予め定めた
軌跡の方向に運動するように両方向の速度指令値により
２つのアームを駆動するようにしたものである。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]

【発明が解決しようとする課題】しかしながら上記２つ
の方式を組合せて使用する場合につぎのような問題点が
ある。図１１（ｂ）のような状態で第１アーム１が反時
計回り方向でストロークエンドだったとすると、第３ア
ーム３と第１アーム１での組合わせでは偏差フィードバ
ック制御ができないという問題がある。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.

【０００６】本発明の目的は、偏差フィードバックを加
えた軌跡制御において最適な一対のアームを選択して軌
跡制御不能状態を極力少なくするようにした多関節作業
機の軌跡制御装置を提供することにある。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]

【課題を解決するための手段】一実施例を示す図１およ
び図４に対応づけて説明すると、本発明は、関節を介し
て回動可能に連結された３本以上のアームと、それらの
アームを駆動するアクチュエータと、各アームの姿勢を
検出する姿勢検出手段１００と、軌跡対象部位の目標軌
跡からのずれ量を演算するずれ量演算手段６０４と、軌
跡制御対象部位の目標軌跡に沿う作業方向の作業速度を
指令する速度指令手段２００と、ずれ量と前記作業速度
とに基づいて目標軌跡に直交する修正方向の修正方向速
度指令値を演算する修正方向速度指令値演算手段６００
と、入力信号に応じて２本のアームを選択し、選択した
２本のアームに応じた拘束条件を出力する拘束条件選択
手段３００（３００Ａ）と、前記拘束条件と検出された
アーム姿勢と作業速度指令値と修正方向速度指令値とに
基づいて、軌跡制御対象部位が目標軌跡上を移動するよ
うに、選択されたアームの回動角速度を演算するアーム
角速度制御値演算手段４００と、演算されたアーム角速
度でアームが回動するように前記アクチュエータを駆動
する駆動制御値演算手段６００とを具備する多関節作業
機の軌跡制御装置に適用される。そして、上述した目的
は、上記拘束条件選択手段３００において、前記作業方
向の移動量が最大となり、前記修正方向には最小となる
アームを選択するとともに、修正方向には最大となり作
業方向には最小となるアームであって、いずれかの回動
方向がストロークエンドではないアームを選択し、選択
した２本のアームに応じた拘束条件を出力することによ
り達成される。また、上記拘束条件選択手段３００Ａに
おいて、前記作業方向の移動量が最大となり、前記修正
方向には最小となるアームを選択するとともに、修正方
向には最大となり作業方向には最小となるアームであっ
て、前記ずれ量によるフィードバック制御を行なわない
場合のアーム回動方向とフィードバック制御を行なう場
合のアーム回動方向とが同一である場合にはその回動方
向でストロークエンドでないアームを選択し、選択した
２本のアームに応じた拘束条件を出力するようにして
も、上記目的を達成できる。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]

【作用】拘束条件選択手段３００は、移動量が修正方向
には最大となり作業方向には最小となる修正方向側アー
ムを選択する場合、いずれかの回動方向がストロークエ
ンドでない修正方向側アームを選択し、その拘束条件を
各速度演算手段４００に入力する。修正方向速度指令値
演算手段６００は、軌跡制御対象部位と目標軌跡との間
の偏差に応じて修正方向速度指令値を演算し、その修正
方向速度指令値と、作業方向速度指令値が角速度指令値
演算手段４００に入力される。そしてその演算結果が制
御手段５００に入力されて一対のアームが演算された角
速度で駆動される。上記拘束条件選択手段３００Ａは、
移動量が修正方向には最大となり作業方向には最小とな
る修正方向側アームを選択する場合、前記ずれ量による
フィードバック制御を行なわない場合のアーム回動方向
とずれ量によるフィードバック制御を行なう場合のアー
ム回動方向とが同一であれば、その方向でストロークエ
ンドでないアームを選択する。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.

【０００９】なお、本発明の構成を説明する上記課題を
解決するための手段と作用の項では、本発明を分かり易
くするために実施例の図を用いたが、これにより本発明
が実施例に限定されるものではない。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]

【実施例】【Example】

−第１の実施例− 図１〜図６により、図７に示す４本アーム式作業機に本
発明を適用する場合について説明する。図７において、
下部走行体ＬＴ上に旋回可能に上部旋回体ＵＳが設けら
れ、これらにより作業機本体ＣＭが構成される。上部旋
回体ＵＳには第１アーム１が回動可能に設けられ、その
先端に第２アーム２が回動可能に設けられ、その先端に
第３アーム３が回動可能に設けられ、その先端に第４ア
ーム４が回動可能に設けられており、各アーム１〜４は
それぞれ油圧シリンダ５〜８により駆動される。第４ア
ーム４の先端には作業用アタッチメント、例えばアース
オーガ掘削ユニット９がピン結合される。図１０に示す
ように、第４アーム４に代えて例えば掘削バケット等の
作業用アタッチメントを取付け、油圧シリンダ８により
回動可能としても良い。この場合は作業用アタッチメン
トＡＴの姿勢角一定制御が行われる。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.

【００１１】図２（ａ）のようにこの作業機の座標を定
義し、以下の説明はこの座標に従う。図２（ａ）に示す
ように、点Ｏを第１アーム１の回動支点とし、第１〜第
４のアーム１〜４の長さをＬ1〜Ｌ4、相対角度をＴ1〜
Ｔ4、各アーム先端の座標を（Ｘ1，Ｙ1）、（Ｘ2，Ｙ
2）、（Ｘ3，Ｙ3）、（Ｘ4，Ｙ4）とするとき、各座標
はそれぞれ、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]

【数１】 Ｘ1＝Ｌ1cosＴ1 …（１） Ｙ1＝Ｌ1sinＴ1 …（２）X1 = L1cosT1 (1) Y1 = L1sinT1 (2)

【数２】 Ｘ2＝Ｘ1＋Ｌ2cos(Ｔ1-Ｔ2) …（３） Ｙ2＝Ｙ1＋Ｌ2sin(Ｔ1-Ｔ2) …（４）X2 = X1 + L2cos (T1-T2) (3) Y2 = Y1 + L2sin (T1-T2) (4)

【数３】 Ｘ3＝Ｘ2＋Ｌ3cos(Ｔ1-Ｔ2-Ｔ3) …（５） Ｙ3＝Ｙ2＋Ｌ3sin(Ｔ1-Ｔ2-Ｔ3) …（６）X3 = X2 + L3cos (T1-T2-T3) (5) Y3 = Y2 + L3sin (T1-T2-T3) (6)

【数４】 Ｘ4＝Ｘ3＋Ｌ4cos(Ｔ1-Ｔ2-Ｔ3-Ｔ4) …（７） Ｙ4＝Ｙ3＋Ｌ4sin(Ｔ1-Ｔ2-Ｔ3-Ｔ4) …（８） と表すことができる。したがって、第４のアーム４先端
の速度Ｘ4V，Ｙ4Vは、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]

【数５】 Ｘ4V＝−Ｔ1V・Ｌ1sinＴ1 −(Ｔ1V-Ｔ2V)・Ｌ2sin(Ｔ1-Ｔ2) −(Ｔ1V-Ｔ2V-Ｔ3)・Ｌ3sin(Ｔ1-Ｔ2-Ｔ3) −(Ｔ1V-Ｔ2V-Ｔ3V-Ｔ4V)・Ｌ4sin(Ｔ1-Ｔ2-Ｔ3-Ｔ4) …（９） ただし、Ｔ1V，Ｔ2V，Ｔ3V，Ｔ4Vはそれぞれ角速度を表
わす。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.

【数６】 Ｙ4V＝Ｔ1V・Ｌ1cosＴ1 ＋(Ｔ1V-Ｔ2V)・Ｌ2cos(Ｔ1-Ｔ2) ＋(Ｔ1V-Ｔ2V-Ｔ3V)・Ｌ3cos(Ｔ1-Ｔ2-Ｔ3) ＋(Ｔ1V-Ｔ2V-Ｔ3V-Ｔ4V)・Ｌ4cos(Ｔ1-Ｔ2-Ｔ3-Ｔ4) …（１０） と表すことができる。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)

【００１４】周知のとおり４本のアームを有する多関節
作業機においては、アーム先端を軌跡制御するには拘束
条件を定めて２本のアームを駆動するが、その組合せは
次に示すように６通りある。 （１）第３，第４のアーム３，４を固定 （第１，第２のアーム１，２で駆動） （２）第２，第４のアーム２，４を固定 （第１，第３のアーム１，３で駆動） （３）第２，第３のアーム３，３を固定 （第１，第４のアーム１，４で駆動） （４）第１，第４のアーム１，４を固定 （第２，第３のアーム２，３で駆動） （５）第１，第３のアーム１，３を固定 （第２，第４のアーム２，４で駆動） （６）第１，第２のアーム１，２を固定 （第３，第４のアーム３，４で駆動）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)

【００１５】さらに第４アーム４を姿勢角一定制御した
場合、次の３通りがある。 （７）第３のアーム３を固定 （第１，第２のアーム１，２で軌跡制御） （８）第２のアーム２を固定 （第１，第３のアーム１，３で軌跡制御） （９）第１のアーム１を固定 （第２，第３のアーム２，３で軌跡制御）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)

【００１６】以上の９通りの拘束条件を式で表すと次の
とおりである。 イ］(1)の場合：Ｔ3V＝０，Ｔ4V＝０ ロ］(2)の場合：Ｔ2V＝０，Ｔ4V＝０ ハ］(3)の場合：Ｔ2V＝０，Ｔ3V＝０ ニ］(4)の場合：Ｔ1V＝０，Ｔ4V＝０ ホ］(5)の場合：Ｔ1V＝０，Ｔ3V＝０ ヘ］(6)の場合：Ｔ1V＝０，Ｔ2V＝０ ト］(7)の場合：Ｔ3V＝０，Ｔ4V＝Ｔ1V−Ｔ2V チ］(8)の場合：Ｔ2V＝０，Ｔ4V＝Ｔ1V−Ｔ3V リ］(9)の場合：Ｔ1V＝０，Ｔ4V＝−（Ｔ2V＋Ｔ3V）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)

【００１７】ここで、(9),(10)式のＸ4V，Ｙ4Vを速度指
令信号ＸV，ＹVとし、各拘束条件毎に上記条件を式
（9),(10)に代入して軌跡制御のために駆動する２本の
アームの角速度ＴnVを求めると、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

【数７】 ＴnV＝ｆn(ＸV,ＹV,Ｔ1,Ｔ2,Ｔ3,Ｔ4) …（１１） となる。ただし、拘束条件（７）〜（９）では軌跡制御
対象部位は第３のアームの先端であるので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,

【数８】 Ｌ4＝０ …（１２） として求める。その詳細な式を図３に示す。なお、図３
中、各種速度は「Ｘドット」あるいは「Ｙドット」のよ
うに示す。L4 = 0 (12) The detailed equation is shown in FIG. Note that FIG.
Medium speeds are indicated as “X dots” or “Y dots”.

【００１８】次に制御装置を図１に基づいて説明する。
制御装置は，角度検出部１００と、速度指令部２００
と、拘束条件選択部３００と、角速度制御値演算部４０
０と、駆動制御値演算部５００と、修正方向速度指令値
演算部６００とが図のように接続されて構成されてい
る。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.

【００１９】角度検出部１００を構成する角度検出器１
０１〜１０４は第１〜第４のアーム１〜４の回動支点に
取付けられ、周知のレバー機構とポテンショメ−タによ
りそれぞれのアームの相対角Ｔ1〜Ｔ4を検出し、各相対
角Ｔ1〜Ｔ4を拘束条件選択部３００、角速度制御値演算
部４００、駆動制御値演算部５００および修正方向速度
指令値演算部６００へ入力する。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.

【００２０】速度指令部２００は、軌跡制御方向の操作
レバー２０２で構成され、例えばレバー機構とポテンシ
ョメ−タによりレバー操作角度に相応した信号を出力
し、この信号を第４アーム４または第３アーム３の先端
の水平および垂直方向の速度指令値ＸV，ＹVとして角速
度制御値演算部４００、符号器２０４および修正方向速
度指令値演算部６００へも入力する。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.

【００２１】修正速度指令値演算部６００は、図４に示
すように構成されており、座標演算部６０１で角度Ｔ１
〜Ｔ４から座標を演算し、軌跡制御対象部位のＸ座標を
出力する。座標記憶部６０２では、速度指令値ＹVから
軌跡制御が開始されたことを検知し、そのときのＸ座標
を記憶し、軌跡制御が終了するまでの値を保持する。そ
のときのＸ座標を軌跡制御対象開始点、換言すると 、
目標とする軌跡の位置Ｘ0とする。したがって、座標演
算部６０１から出力される実際の位置Ｘと座標記憶部６
０２から出力される目標とする軌跡の位置Ｘ0との偏差
ΔＸを加算点６０４で算出して修正速度演算部６０３に
入力する。修正速度演算部６０３では、次に示す式（1
3）から偏差ΔＸと速度指令値ＹVから速度指令値ＸVと
を算出し、これを角速度制御値演算部４００へ入力す
る。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.

【数９】 ＸV＝ｋ・ΔＸ・｜ＹV｜ …（１３） ただし、ｋは定数XV = k · ΔX · | YV | (13) where k is a constant

【００２２】拘束条件選択部３００は、角度Ｔ1〜Ｔ4，
符号器２０４から出力される軌跡制御方向の速度指令値
の符号，およびモード設定器３０１の出力から、後述す
る処理手順によって拘束条件（１）〜（９）を選択出力
し、これを角速度制御値演算部４００へ入力する。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.

【００２３】ここで、モード設定器３０１は、オペレー
タの操作により、第４アーム４の相対角度が所定値にな
ると自動的に姿勢角一定制御を行うモードを選択するも
のである。その姿勢角一定制御モードの選択時、第４の
アーム４の姿勢角が所定角、例えば−９０°になると、
上記９つの拘束条件の中から姿勢角一定制御を含む拘束
条件を優先して選択する。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.

【００２４】角速度制御値演算部４００は、拘束条件に
よって演算ブロック４０１〜４０９の中から対応するい
ずれか１つの演算ブロックを選択し、角度Ｔ1〜Ｔ4およ
び速度指令値ＸV，ＹVから、拘束条件によって選択され
たアームの角速度制御値ＴnVを演算し、これらを駆動制
御値演算部５００へ入力する。なお、各演算ブロック４
０１〜４０９の出力は同一アームの制御値毎に各アーム
の加算点４１０〜４２２で加算され出力される。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.

【００２５】駆動制御値演算部５００は、次の式に基づ
いて角速度制御値ＴnVおよび角度Ｔ1〜Ｔ4から、シリン
ダ５〜８の流量制御値Ｑ1〜Ｑ4を演算し、それを電気油
圧変換弁１１〜１４に入力する。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.

【数１０】 Ｑｎ＝ＴnV・ｇｎ（Ｔｎ）・Ａｎ …（１４） ここで、ｇｎ（Ｔｎ）はリンク補正係数 Ａｎはシリンダ受圧面積Qn = TnV ・ gn (Tn) ・ An (14) where gn (Tn) is a link correction coefficient An is a cylinder pressure receiving area

【００２６】これら電気油圧変換弁１１〜１４には油圧
源から圧油が導かれており、入力される流量制御値Ｑ1
〜Ｑ4に応じた流量および方向で圧油を第１〜第４アー
ム１〜４用のシリンダ５〜８に供給し、軌跡制御が行わ
れる。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.

【００２７】拘束条件選択部３００において、制御すべ
きアームの選択は図５に示す処理手順により行われる。
ステップＳ２０においてモード設定器３０１の出力を読
み込み、第４アーム４の姿勢角一定制御モードか否かを
判定する。姿勢角一定制御モードが選択されていなけれ
ばステップＳ２へ、選択されているとステップＳ１へ進
む。ステップＳ１では、第４アーム４の相対角度が予め
定めた所定角か否かを判定し、所定角に達していなけれ
ばステップＳ２に進み、所定角に達していればステップ
Ｓ３へ進む。ステップＳ２では、図２（ａ）に示す各ア
ームの回動支点と第４アーム４先端を結ぶ線分がＸ軸と
なす角（制御角と呼ぶ）Ａ14，Ａ24，Ａ34，Ａ44を計算
する。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.

【００２８】前述の各アーム先端の座標を用いると、Using the coordinates of the tip of each arm described above,

【数１１】 Ａ14＝｜tan^-1(Ｙ4/Ｘ4)｜ …（１５）A14 = | tan ⁻¹ (Y4 / X4) | (15)

【数１２】 Ａ24＝｜tan^-1｛(Ｙ4−Ｙ1)/(Ｘ4−Ｘ1)｝｜ …（１６）A24 = | tan ^-1 {(Y4-Y1) / (X4-X1)} | (16)

【数１３】 Ａ34＝｜tan^-1｛(Ｙ4−Ｙ2)/(Ｘ4−Ｘ2)｝｜ …（１７）A34 = | tan ^-1 {(Y4-Y2) / (X4-X2)} | (17)

【数１４】 Ａ44＝｜tan^-1｛(Ｙ4−Ｙ3)/(Ｘ4−Ｘ3)｝｜ …（１８） と表すことができる。A44 = | tan ^-1 {(Y4-Y3) / (X4-X3)} | (18)

【００２９】同様にステップＳ３では図２（ｂ）に示す
各アームの回動支点と第３アーム３先端を結ぶ線分がＸ
軸となす各（制御角）Ａ13，Ａ23，Ａ33を計算する。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.

【数１５】 Ａ13＝｜tan^-1(Ｙ3/Ｘ3)｜ …（１９）A13 = | tan ⁻¹ (Y3 / X3) | (19)

【数１６】 Ａ23＝｜tan^-1｛(Ｙ3−Ｙ2)/(Ｘ3−Ｘ2)｝｜ …（２０）A23 = | tan ^-1 {(Y3-Y2) / (X3-X2)} | (20)

【数１７】 Ａ33＝｜tan^-1｛(Ｙ3−Ｙ2)/(Ｘ3−Ｘ2)｝｜ …（２１）A33 = | tan ^-1 {(Y3-Y2) / (X3-X2)} | (21)

【００３０】次にステップＳ４では、各アームの制御角
を比べ、最も０°に近い制御角のアームを選択する。こ
うして選択されたアームは、その回動によって生じる軌
跡制御対象部位の移動量がＸ軸方向には最小となりＹ軸
方向には最大となるアームであり、言い換えればＹ軸方
向の制御に最も適したアームである。ステップＳ５で
は、ステップＳ４で選択したアームが制御方向でストロ
ークエンドとなっていないかを判定する。この判定は図
６に示すように、たとえば第１アームの場合、速度指令
値ＹVが負のときは当該アームの相対角が予め定めた最
小値（min）より小さいか否か、あるいはＹVが正のとき
は予め定めた最大値（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).

【００３１】ストロークエンドの場合はステップＳ６に
進み、未選択のアーム数が１本か否かを判定する。１本
の場合はアームを２本選択することができないのでステ
ップＳ１３へ進み制御不可とする。すなわち拘束条件を
出力しない。未選択のアーム数が２本以上の場合はステ
ップＳ７へ進み、ステップＳ５でストロークエンドとな
ったアームを除いた中から制御角が最も０°に近いアー
ムを選択し、同様にストロークエンドを判定するためス
テップＳ５へ戻る。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.

【００３２】ステップＳ５でストロークエンドでない場
合はステップＳ８へ進み、各アームの制御角を比べ、最
も９０°に近い制御角のアームを選択する。ここで選択
されたアームは、その回動によって生じる軌跡制御対象
部位の移動量がＸ軸方向に最大となりＹ軸方向には最小
となるアームであり、言い換えればＸ軸方向の制御に最
も適したアームである。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.

【００３３】ステップＳ９では、ステップＳ８で選択し
たアームが時計回り方向および反時計回り方向のいずれ
か一方がストロークエンドになっているかを判定する。
いずれの回動方向ともストロークエンドでない場合は、
ステップＳ４で選択したアームと合わせてアームが２本
選択されたことになり、ステップＳ１２へ進んで選択し
たアームに対応する拘束条件（１）〜（９）のいずれか
を出力する。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.

【００３４】ステップＳ９でいずれか一方の回動方向が
ストロークエンドと判定された場合はステップＳ１０で
未選択のアーム数を調べ、残っていない場合はアームを
２本選択することができないのでステップＳ１３へ進み
制御不可とする。未選択のアームが残っている場合はス
テップＳ１１へ進み、ステップＳ９でストロークエンド
となったアームを除いた中から制御角が最も９０°に近
いアームを選択し、同様にストロークエンドを判定する
ためステップＳ９へ戻る。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.

【００３５】この手順は、垂直軌跡制御を想定したもの
であるが、水平軌跡制御では、ステップＳ４，ステップ
Ｓ７とステップＳ８，１１を入れ替えればよい。This procedure assumes vertical trajectory control. In horizontal trajectory control, steps S4 and S7 and steps S8 and S11 may be interchanged.

【００３６】次に本装置の動作について説明する。図示
しない電源スイッチを投入するとこの装置が起動し、角
度検出器１０１〜１０４で検出された角度Ｔ1〜Ｔ4，軌
跡制御レバー２０２で指令された速度指令値ＹVの符号
およびモード設定器３０１で設定されたモードに基づい
て、拘束条件選択部３００は、図５の手順により軌跡制
御に最も適した２本のアームを選択し、選択されたアー
ムに対応する拘束条件を出力する。修正方向速度指令値
演算部６００では、速度指令値ＹVと角度Ｔ1〜Ｔ4から
偏差修正を加えた修正方向速度指令値ＸVを演算して出
力する。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.

【００３７】角速度制御値演算部４００では、選択され
た拘束条件に対応するいずれかの演算ブロックが選択さ
れ、角度Ｔ1〜Ｔ4および速度指令値ＸV，ＹVから、選択
されたアームの角速度制御値ＴnVが演算される。選択さ
れていない演算ブロックの角速度制御値ＴnVは零なの
で、各演算ブロック出力のうち同一アームごとにそのア
ーム角速度制御値を加算点４１０〜４２０で加算すれ
ば、選択された一対のアームの角速度制御値の切換えが
行われる。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.

【００３８】これらの角速度制御値Ｔ1V〜Ｔ4Vは駆動制
御値演算部５００でリンク補正され、各アームの流量制
御値Ｑ1〜Ｑ4に変換される。これらの流量制御値Ｑ1〜
Ｑ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.

【００３９】このようにストロークエンド判定方法をス
テップＳ９のように制御方向に関してストロークになっ
ていないものを選択する従来方法から、制御方向にかか
わらず両回動方向のいずれもストロークエンドになって
いないものを選択する方法に変えることにより、図１１
（ｂ）で示す第１のアーム１のように、偏差がでていな
いときは時計回り方向に回動し偏差によっては反時計回
り方向に回動するようなアームの場合でも、両回動方向
がともにストロークエンドでないアームが選択されるの
で、偏差フィードバック制御を適用する場合においても
制御不能となることがない。すなわち、従来方式では、
修正側アームとして制御方向がストロークエンドではな
いとして選択されたアームでも、修正のための回動方向
にはストロークエンドである場合があり、その場合には
制御が不能となることがあったが、本実施例によれば、
修正のための回動方向がストロークエンドにあるアーム
は事前に除かれるから、そのような制御不能に陥ること
がない。また、アームの姿勢を参照するだけでストロー
クエンドの判定ができるので演算時間が短縮できる。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.

【００４０】−第２の実施例− 次に図８および図９により本発明による多関節作業機の
軌跡制御装置の他の実施例を説明する。第１の実施例で
は時計回り方向および反時計回り方向の両回動方向がと
もにストロークエンドではないアームが選択されないよ
うにしたが、第２の実施例では、上述の偏差フィードバ
ック制御をせずに軌跡制御する場合のアーム回動方向
と、偏差フィードバック制御を行って軌跡制御する場合
のアーム回動方向とが同一か否かを判定し、同一であれ
ば前者の方向がストロークエンドであれば後者の方向が
ストロークエンドか否かを調べるまでもなく、そのアー
ムを選択しないようにしたものである。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.

【００４１】図８と図９において、図１および図５と同
一の箇所には同一の符号を付して相違点を主に説明す
る。修正方向速度指令値演算部６００で演算された修正
方向速度指令値ＸVは拘束条件選択部３００Ａに入力さ
れる。拘束条件選択部３００Ａは、図９のフローチャー
トにより制御すべきアームを選択する。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.

【００４２】図９のフローチャートにおいて、ステップ
Ｓ８で最も９０°に近い制御角のアームを選択した後、
ステップＳ２１で修正方向速度指令値ＸVを読み込む。
次いでステップＳ２２において、修正方向と制御方向が
同一が否かを判定し、異なっていればステップＳ１０に
進み、同一であればステップＳ２３に進む。ここで、ス
テップＳ２２では、偏差フィードバックを行なわずに軌
跡制御する場合のアーム制御回動方向と、読み込まれた
修正方向速度指令値ＸVからわかる修正回動方向とが一
致しているかを判定する。ステップＳ２３では制御回動
方向がストロークエンドか否かを判定し、ストロークエ
ンドであればステップＳ１０に進み、ストロークエンド
でなければステップＳ１２に進む。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.

【００４３】ステップＳ１０では、未選択のアーム数を
調べ、残っていない場合にはアームを２本選択できない
のでステップＳ１３へ進み、制御不可とする。未選択の
アームが残っていれば、ステップＳ１１に進み、ステッ
プＳ２２で同一方向でなかったアームとステップＳ２３
でストロークエンドとなったアームを除いた中から制御
角が最も９０°に近いアームを選択しステップＳ２２に
戻る。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.

【００４４】先に説明した第１の実施例では、いずれか
一方のアーム回動方向がストロークエンドであるアーム
は選択されなかった。換言すると両回動方向ともにスト
ロークエンドでないアームだけが選択された。しかし、
第２の実施例では、制御回動方向と修正回動方向が同一
である場合には、その回動方向さえストロークエンドで
なければそのアームが選択されるから、アームを有効に
利用できる。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.

【００４５】なお、拘束条件選択部３００をソフトウエ
アの形態で構成したが、予め角度Ｔ1〜Ｔ4，速度指令値
ＸV，ＹVの符号およびモード設定信号の種々の組合せに
対する拘束条件を計算しておき、それらの組合せに対す
る拘束条件をテーブル化してもよい。この場合、入力デ
ータでテーブルを参照すれば直ちに拘束条件が求まり図
５，９の処理時間の短縮が可能である。また、拘束条件
に対応する演算ブロックを選択して角速度制御値を演算
すれば良いので、すべてのアームの組合せに対して角速
度制御値を演算する必要がなく、処理時間はアーム数が
増えても変わらない。さらに、モード設定器３０１を設
けることにより、容易に姿勢角一定制御への切換えを行
うことができる。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.

【００４６】また、各演算ブロックの角速度制御値を同
一アーム毎に加算しているので、拘束条件の切換えが自
動的に行われる。なお、この切換に際して、旧データを
漸減し新データを漸増して切換時のショックを緩和する
こともできる。さらにまた、姿勢角一定制御モードを選
択すれば、第４のアーム４が予め定めた所定値、例えば
−９０°になると自動的に姿勢角一定制御が行われるの
で、小径の垂直孔などへ作業用アタッチメントを移動さ
せる際などに孔と先端アームとの接触を防止できる。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.

【００４７】なお、本発明を適用するにあたっては以上
の実施例の各構成要素を次のようにしても良い。 アーム数は４本に限定されない。 各アームを油圧シリンダで駆動したが、油圧に限定さ
れず、また油圧モータ、油圧ロータリアクチュエータな
どその他のアクチュエータを用いることができる。 バイブロハンマ，アースオーガや掘削バケット等に使
用できる旨述べたが、その他の各種作業用アタッチメン
トにも使用できる。 第１アーム１の角度を上部旋回体に対する相対角で検
出したが、第１アームの対地角を検出しても良く、ま
た、作業機本体の傾斜角を検出して相対角を補正しても
良い。 角度検出器として、磁気抵抗素子を用いたもの、差動
コイルを用いたもの、光学式、磁気式のロータリエンコ
ーダを用いたものなどポテンショメ−タに限定されな
い。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]

【発明の効果】本発明によれば次のような効果が得られ
る。すなわち、一対の駆動アームを選択して軌跡制御対
象部位を軌跡制御駆動する際、軌跡制御対象部位の目標
軌跡からのずれ量をアームの角速度指令値にフィードバ
ック制御する軌跡制御装置において、一対のアームを選
択する際に、制御方向がストロークエンドであるアーム
を排除する従来方式では、制御方向はストロークエンド
でなく選択されたアームでも修正方向にストロークエン
ドである場合には、偏差フィードバックができず、制御
不能状態に陥ってしまったが、本発明のように、軌跡制
御のために駆動される一対のアームを選択する際に、ア
ームの両回動方向ともストロークエンドでないアームを
選択するようにしたので、偏差フィードバック制御中に
制御不能に陥らなくなる。したがって、上記偏差フィー
ドバック制御方式で軌跡制御する場合でも、作業機のア
ーム数や姿勢によらず軌跡制御に最も適した２本のアー
ムの組を自動的に選択することが可能となる。また、ず
れ量によるフィードバック制御を行なわない場合のアー
ム回動方向とずれ量によるフィードバック制御を行なう
場合のアーム回動方向とが同一か否かを判定し、同一で
あれば制御方向のみストロークエンドであることを判定
するようにしてアームを選択する方式でも上記と同様な
作用効果が得られるとともに、アームを有効に利用でき
る。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.

【図面の簡単な説明】[Brief description of the drawings]

【図１】本発明の一実施例の構成図FIG. 1 is a configuration diagram of an embodiment of the present invention.

【図２】座標を定義する図FIG. 2 is a diagram for defining coordinates.

【図３】拘束条件の詳細を示す図FIG. 3 is a diagram showing details of constraint conditions.

【図４】修正方向速度指令値演算手段の詳細ブロック図FIG. 4 is a detailed block diagram of a correction direction speed command value calculating means.

【図５】拘束条件選択部の処理手順を示す図FIG. 5 is a diagram showing a processing procedure of a constraint condition selecting unit.

【図６】ストロークエンドの判定条件を説明する図FIG. 6 is a view for explaining conditions for determining a stroke end.

【図７】本発明が適用される作業機の一実施例の側面図FIG. 7 is a side view of an embodiment of a working machine to which the present invention is applied.

【図８】本発明の他の実施例の構成図FIG. 8 is a configuration diagram of another embodiment of the present invention.

【図９】拘束条件選択部の処理手順を示す図FIG. 9 is a diagram showing a processing procedure of a constraint condition selecting unit.

【図１０】本発明が適用される作業機の他の例の側面図FIG. 10 is a side view of another example of a working machine to which the present invention is applied.

【図１１】従来の問題点を説明する図FIG. 11 is a diagram illustrating a conventional problem.

【符号の説明】[Explanation of symbols]

１〜４：アーム ５〜８：油圧シリンダ １１〜１４：電気油圧変換弁 １００：角度検出手段 １０１〜１０４：角度検出器 ２００：速度指令部 ２０２：速度指令レバー 300,300A：拘束条件選択部 ３０１：モード設定器 ４００：角速度制御値演算部 ４０１〜４０９：演算ブロック ５００：駆動制御値演算部 ６００：修正方向速度指令値演算手段 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.⁷，ＤＢ名) G05B 19/18 - 19/46 E02F 3/43,9/20 ──────────────────────────────────────────────────続 き Continued on the front page (58) Field surveyed (Int.Cl. ⁷ , DB name) G05B 19/18-19/46 E02F 3 / 43,9 / 20

Claims (2)

(57)【特許請求の範囲】(57) [Claims] 【請求項１】 関節を介して回動可能に連結された３本
以上のアームと、 それらのアームを駆動するアクチュエータと、 各アームの姿勢を検出する姿勢検出手段と、 軌跡対象部位の目標軌跡からのずれ量を演算するずれ量
演算手段と、 軌跡制御対象部位の目標軌跡に沿う作業方向の作業速度
を指令する速度指令手段と、 前記ずれ量と前記作業速度とに基づいて目標軌跡に直交
する修正方向の修正方向速度指令値を演算する修正方向
速度指令値演算手段と、 前記作業方向の移動量が最大となり、前記修正方向には
最小となるアームであって制御方向にストロークエンド
ではないアームを選択するとともに、修正方向には最大
となり作業方向には最小となるアームであって、いずれ
かの回動方向がストロークエンドではないアームを選択
し、選択した２本のアームに応じた拘束条件を出力する
拘束条件選択手段と、 前記拘束条件と検出されたアーム姿勢と作業速度指令値
と修正方向速度指令値とに基づいて、前記軌跡制御対象
部位が前記目標軌跡上を移動するように、選択されたア
ームの回動角速度を演算するアーム角速度制御値演算手
段と、 演算されたアーム角速度でアームが回動するように前記
アクチュエータを駆動する駆動制御値演算手段とを具備
すること特徴とする多関節作業機の軌跡制御装置。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. 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.