JPH07200015A - 多関節作業機の軌跡制御装置 - Google Patents

Abstract

(57)【要約】 【目的】 偏差フィードバックを加えた軌跡制御におい
て最適な一対のアームを選択して軌跡制御不能状態を極
力少なくする。 【構成】 一対の駆動アームを選択して軌跡対象部位を
軌跡制御駆動する際、軌跡対象部位の目標軌跡からのず
れ量をアームの角速度指令値にフィードバック制御する
軌跡制御装置において、軌跡制御のために駆動される一
対のアームのうち偏差フィードバックに使用されるアー
ムを選択する条件として、アームの時計回り方向および
反時計回り方向がともストロークエンドでないアームを
選択する。

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

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

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

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

【０００５】

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

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

【０００７】

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

【０００８】

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

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

【００１０】

【実施例】

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

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

【００１２】

【数１】 Ｘ1＝Ｌ1cosＴ1 …（１） Ｙ1＝Ｌ1sinＴ1 …（２）

【数２】 Ｘ2＝Ｘ1＋Ｌ2cos(Ｔ1-Ｔ2) …（３） Ｙ2＝Ｙ1＋Ｌ2sin(Ｔ1-Ｔ2) …（４）

【数３】 Ｘ3＝Ｘ2＋Ｌ3cos(Ｔ1-Ｔ2-Ｔ3) …（５） Ｙ3＝Ｙ2＋Ｌ3sin(Ｔ1-Ｔ2-Ｔ3) …（６）

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

【００１３】

【数５】 Ｘ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はそれぞれ角速度を表
わす。

【数６】 Ｙ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) …（１０） と表すことができる。

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

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

【００１６】以上の９通りの拘束条件を式で表すと次の
とおりである。 イ］(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）

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

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

【数８】 Ｌ4＝０ …（１２） として求める。その詳細な式を図３に示す。なお、図３
中、各種速度は「Ｘドット」あるいは「Ｙドット」のよ
うに示す。

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

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

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

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

【数９】 ＸV＝ｋ・ΔＸ・｜ＹV｜ …（１３） ただし、ｋは定数

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

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

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

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

【数１０】 Ｑｎ＝ＴnV・ｇｎ（Ｔｎ）・Ａｎ …（１４） ここで、ｇｎ（Ｔｎ）はリンク補正係数 Ａｎはシリンダ受圧面積

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

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

【００２８】前述の各アーム先端の座標を用いると、

【数１１】 Ａ14＝｜tan^-1(Ｙ4/Ｘ4)｜ …（１５）

【数１２】 Ａ24＝｜tan^-1｛(Ｙ4−Ｙ1)/(Ｘ4−Ｘ1)｝｜ …（１６）

【数１３】 Ａ34＝｜tan^-1｛(Ｙ4−Ｙ2)/(Ｘ4−Ｘ2)｝｜ …（１７）

【数１４】 Ａ44＝｜tan^-1｛(Ｙ4−Ｙ3)/(Ｘ4−Ｘ3)｝｜ …（１８） と表すことができる。

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

【数１５】 Ａ13＝｜tan^-1(Ｙ3/Ｘ3)｜ …（１９）

【数１６】 Ａ23＝｜tan^-1｛(Ｙ3−Ｙ2)/(Ｘ3−Ｘ2)｝｜ …（２０）

【数１７】 Ａ33＝｜tan^-1｛(Ｙ3−Ｙ2)/(Ｘ3−Ｘ2)｝｜ …（２１）

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

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

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

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

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

【００３５】この手順は、垂直軌跡制御を想定したもの
であるが、水平軌跡制御では、ステップＳ４，ステップ
Ｓ７とステップＳ８，１１を入れ替えればよい。

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

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

【００３８】これらの角速度制御値Ｔ1V〜Ｔ4Vは駆動制
御値演算部５００でリンク補正され、各アームの流量制
御値Ｑ1〜Ｑ4に変換される。これらの流量制御値Ｑ1〜
Ｑ4は電気油圧変換弁１１〜１４に供給され、油圧源か
らの圧油が所定方向、所定流量にて第１〜第４シリンダ
５〜８に供給される。これにより選択されたアームが回
動して第４アーム４先端の軌跡が速度指令レバーの操作
に応じて制御される。あるいは、第３アーム３先端の軌
跡が速度指令レバーの操作に応じて制御され、第４アー
ム４の姿勢角が一定に制御される。

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

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

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

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

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

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

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

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

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

【００４８】

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

【図面の簡単な説明】

【図１】本発明の一実施例の構成図

【図２】座標を定義する図

【図３】拘束条件の詳細を示す図

【図４】修正方向速度指令値演算手段の詳細ブロック図

【図５】拘束条件選択部の処理手順を示す図

【図６】ストロークエンドの判定条件を説明する図

【図７】本発明が適用される作業機の一実施例の側面図

【図８】本発明の他の実施例の構成図

【図９】拘束条件選択部の処理手順を示す図

【図１０】本発明が適用される作業機の他の例の側面図

【図１１】従来の問題点を説明する図

【符号の説明】 １〜４：アーム ５〜８：油圧シリンダ １１〜１４：電気油圧変換弁 １００：角度検出手段 １０１〜１０４：角度検出器 ２００：速度指令部 ２０２：速度指令レバー 300,300A：拘束条件選択部 ３０１：モード設定器 ４００：角速度制御値演算部 ４０１〜４０９：演算ブロック ５００：駆動制御値演算部 ６００：修正方向速度指令値演算手段

Claims (2)

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