JPH0796193B2 - 多関節型アームの伸縮制御装置 - Google Patents

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は、第１アクチュエータによって上下揺動駆動自
在な第１アームが設けられ、先端部に作業用ハンドを備
えた第２アームが、前記第１アームの先端部に対して第
２アクチュエータによって上下揺動駆動自在に枢支さ
れ、前記作業用ハンドを直線状の設定軌跡に沿って移動
させるように、前記第１アクチュエータ及び前記第２ア
クチュエータ夫々に対する単位時間当たりの目標駆動量
を設定時間毎に求める目標駆動量演算手段と、その目標
駆動量演算手段にて求められた目標駆動量の情報に基づ
いて前記第１アクチュエータ及び前記第２アクチュエー
タの夫々を駆動する駆動手段とが設けられた多関節型ア
ームの伸縮制御装置に関する。

〔従来の技術〕

上記この種の多関節型アームの伸縮制御装置は、多関節
型アームの先端部に設けられた作業用ハンドを直線状の
設定軌跡に沿って移動させるように、アーム駆動をフィ
ードフォワード制御するようにしたものである。

ところで、アーム駆動をフィードフォワード制御する場
合には、制御パラメータとなるアクチュエータの動作特
性、特に、粘性抵抗情報の変動が制御精度に大きく影響
する。

そこで、従来では、駆動手段を、目標駆動量演算手段か
ら与えられる目標駆動量と、駆動結果とに基づいてフィ
ードバック制御させるように構成して、フィードフォワ
ード制御における制御誤差を補正させるようにしていた
（例えば、本出願人が先に提案した特願昭63−180601号
参照）。

〔発明が解決しようとする課題〕

従来構成では、粘性抵抗情報の変動による制御精度の誤
差を、フィードバック制御で補正させるようにしていた
ので、目標駆動量を、アクチュエータを駆動可能な最大
駆動量に設定することは困難であり、アクチュエータを
駆動可能な最高速度で作業用ハンドを移動させることは
できなかった。

ちなみに、予めアームを駆動して粘性抵抗による誤差を
補正させるチューニングを行った後、実際の作業を開始
させるようにすることも考えられるが、実際の作業に無
関係な動作を行わせることになり、作業能率が低下する
不利がある。

本発明は、上記実情に鑑みてなされたものであって、そ
の目的は、作業能率の低下を抑制しながら、作業用ハン
ドを高速移動させることができるようにすることにあ
る。

〔課題を解決するための手段〕

本発明にかかる多関節型アームの伸縮制御装置は、上記
目的を達成するために、第１アクチュエータによって上
下揺動駆動自在な第１アームが設けられ、先端部に作業
用ハンドを備えた第２アームが、前記第１アームの先端
部に対して第２アクチュエータによって上下揺動駆動自
在に枢支され、前記作業用ハンドを直線状の設定軌跡に
沿って移動させるように、前記第１アクチュエータ及び
前記第２アクチュエータ夫々に対する単位時間当たりの
目標駆動量を設定時間毎に求める目標駆動量演算手段
と、その目標駆動量演算手段にて求められた目標駆動量
の情報に基づいて前記第１アクチュエータ及び前記第２
アクチュエータの夫々を駆動する駆動手段とが設けられ
たものであって、その特徴構成は以下の通りである。

すなわち、前記目標駆動量演算手段は、前記第１アクチ
ュエータ及び前記第２アクチュエータのうち駆動負荷の
大きいがわの初期の駆動量を最大駆動量より予め設定さ
れた低い駆動量を基準目標駆動量として、該基準目標駆
動量で駆動する駆動負荷の大きいがわの実際の駆動量を
検出してその検出結果に基づいて駆動負荷の小さいがわ
の単位時間当たりの目標駆動量を補正するとともに、駆
動負荷の大きいがわにおける目標駆動量を求めるための
記憶された粘性抵抗情報を駆動結果に基づいて設定時間
毎に補正するように構成され、且つ、前記設定軌跡に沿
う移動を行う毎に、前記目標駆動量を求めるために記憶
された前記基準目標駆動量を漸次増大するように構成さ
れている点にある。

〔作 用〕

つまり、粘性抵抗の変動によるフィードフォワード制御
の誤差は、駆動手段によるフィードバック制御で補正で
きることから、チューニングしながら作業用ハンドを移
動させても、その目標移動軌跡に対する誤差を小さくす
ることはできる。

そこで、第１アクチュエータ及び第２アクチュエータの
うち、駆動負荷の大きいがわのアクチュエータの実際の
駆動量を検出してその検出結果に基づいて駆動負荷の小
さいがわのアクチュエータの単位時間当たりの目標駆動
量を補正し、かつ、粘性抵抗情報を補正させるチューニ
ングを行いながら、実際に作業用ハンドを移動させ、そ
して、設定軌跡に沿う移動を行う毎に、作業用ハンドの
移動速度が増大するように、基準目標駆動量を漸次増大
させるのである。

〔発明の効果〕

従って、作業を行いながらチューニングして、アクチュ
エータを駆動可能な最高速度で駆動する状態となるよう
に、目標駆動量を漸次増大することができるので、作業
能率が低下しないようにしながら、作業用ハンドを高速
移動させることができるに至った。

〔実施例〕

以下、本発明を果実収穫用の作業機に適用した場合にお
ける実施例を図面に基づいて説明する。

第７図に示すように、左右一対の走行車輪（１）を前後
に備えた車体（Ｖ）に、ブーム（２）が昇降並びに旋回
自在に取り付けられ、そのブーム（２）の先端部に、補
助ブーム（３）が水平方向に揺動自在に取り付けられ、
そして、果実収穫用の作業用ハンド（Ｈ）を備えた作業
用マニプレータ（４）が、前記補助ブーム（３）の先端
部に取り付けられ、もって、果実収穫用の作業機が構成
されている。尚、図中、（５）はブーム昇降用油圧シリ
ンダ、（６）はブーム旋回用油圧シリンダ、（７）は補
助ブーム揺動用油圧シリンダである。

前記作業用マニプレータ（４）は、多関節型アーム
（８）と、そのアーム（８）の先端部に取り付けられた
作業用ハンド（Ｈ）とからなる。

第１図及び第７図に示すように、前記多関節型アーム
（８）は、前記補助ブーム（３）の先端部において旋回
用電動モータ（9a）にて縦軸芯（Ｙ）周りで旋回駆動さ
れる基端部ベース（8a）と、その基端部ベース（8a）に
対して第１アクチュエータとしての揺動用電動モータ
（9b）にて横軸芯（Ｘ）周りで揺動駆動される第１アー
ム（8b）と、その第１アーム（8b）の先端部に対して第
２アクチュエータとしての伸縮用電動モータ（9b）によ
って上下揺動駆動自在に枢支された第２アーム（8c）と
からなる。尚、前記作業用ハンド（Ｈ）は、前記伸縮用
電動モータ（9c）による前記第２アーム（8c）の揺動に
連動して揺動駆動されるように、前記第２アーム（8c）
の先端部に取り付けられている。

そして、側面視において前記第１アーム（8b）と前記第
２アーム（8c）とが略重なる状態を初期位置として、前
記作業用ハンド（Ｈ）が、前記初期位置と収穫対象果実
（Ｆ）とを結ぶ直線状の設定軌跡（Ｌ）に沿って移動す
るように、前記揺動用電動モータ（9b）及び前記伸縮用
電動モータ（9c）夫々の目標駆動量（Duty₁），（Dut
y₂）を、設定時間（Δｔ）毎に求めながら駆動して、前
記アーム（８）を伸縮させるようになっている。

但し、前記第１アーム（8b）と前記第２アーム（8c）と
は同じ長さ（Ａ）に形成され、前記第１アーム（8b）
は、前記揺動用電動モータ（9b）にて直接的に揺動駆動
されるようになっている。一方、前記第２アーム（8c）
は、前記第１アーム（8b）の基端部側に取り付けられた
前記伸縮用電動モータ（9c）に連動連結されたチェーン
（10a）によって、その基端部を前記第１アーム（8b）
の先端部において回動させるように構成されている。そ
して、前記作業用ハンド（Ｈ）は、前記伸縮用電動モー
タ（9c）による前記第２アーム（8c）の揺動に連動し
て、前記第２アーム（8c）の揺動角度（θ_２）の半分の
角度を、前記第２アーム（8c）の揺動方向とは逆方向に
向けて揺動されるように、それらの枢支点同士がチェー
ン（10b）にて連動連結されている。

従って、前記作業用ハンド（Ｈ）を前記設定軌跡（Ｌ）
に沿って移動させるためには、前記第１アーム（8b）に
対する前記第２アーム（8c）の揺動角度（θ_２）の半分
から前記第１アーム（8b）の鉛直方向に対する揺動角度
（θ_１）を減算した角度が、前記設定軌跡（Ｌ）の水平
方向に対する仰角（η）に一致するように、前記揺動用
電動モータ（9b）及び前記伸縮用電動モータ（9c）夫々
の単位時間当たりの目標駆動量（Duty₁），（Duty₂）の
夫々が設定されることになる。

尚、前記揺動用電動モータ（9b）及び前記伸縮用電動モ
ータ（9c）夫々の駆動を制御するための制御構成につい
ては後述する。

前記作業用ハンド（Ｈ）について説明すれば、第６図に
示すように、その先端部に、作業対象としての果実
（Ｆ）を吸着するバキュームパッド（11）が設けられ、
そのバキュームパッド（11）に吸着された果実（Ｆ）を
覆うように、前記バキュームパッド（11）の通気管（1
2）に対して前後方向に出退自在に外嵌支持された捕捉
部ケース（13）と、その捕捉部ケース（13）内に取り込
んだ果実（Ｆ）の柄部分を上下方向に挟み込んだ状態で
切断する柄切断装置（14）とを備えている。尚、詳述は
しないが、前記バキュームパッド（11）は、前記通気管
（12）内に付設されるエジェクタポンプ（図示せず）等
を利用した負圧発生装置によって吸引されることにな
る。

前記捕捉部ケース（13）は、前記通気管（12）に外嵌さ
れる本体部（13a）と、その本体部（13a）に対してハン
ド内側方向に向けて復帰付勢された状態で、且つ、上下
左右方向に向けて夫々開閉自在に枢着されたカバー（13
b）とからなる。

但し、詳述はしないが、前記カバー（13b）は、上下左
右に４分割され、ハンド前方側に前記バキュームパッド
（11）が吸着された果実が通過する開口が形成されるよ
うになっている。つまり、前記開口を通して果実が前記
捕捉部ケース（13）内に取り込まれるときに、事実の径
が前記開口よりも大である場合には４分割されたカバー
の夫々がハンド外側方向に開いて前記開口よりも径が大
なる果実を取り込むことができるようになっているので
ある。尚、４分割されたカバー（13b）のうちの下側に
位置するものは、アクチュエータ（図示せず）によって
下方側に向けて開き操作されるように構成され、前記捕
捉部ケース（13）内に保持される柄を切断された果実
（Ｆ）を外部に排出できるようになっている。

前記柄切断装置（14）は、前記果実（Ｆ）の柄を切断す
るバリカン型の刃体（14a）と、その刃体（14a）に対し
て前記果実（Ｆ）の柄を下方側から押圧支持する柄支持
部材（14b）とを備えている。

但し、詳述はしないが、前記作業用ハンド（Ｈ）には、
前記捕捉部ケース（13）を出退操作するためのアクチュ
エータや前記柄切断装置（14）を駆動するためのアクチ
ュエータ等の収穫作動用の各種アクチュエータや、前記
バキュームパッド（11）に果実（Ｆ）を吸着したことを
検出するためのセンサ、前記捕捉部ケース（13）の出退
位置を検出するためのセンサ、及び、前記柄切断装置
（14）の駆動状態を検出するためのセンサ等の各種セン
サが設けられることになる。

尚、第６図中、（S₁）は前記作業用ハンド（Ｈ）が向い
た方向に位置する果実（Ｆ）を撮像する撮像手段として
のカラー式のイメージセンサ、（S₂）は前記作業用ハン
ド（Ｈ）が収穫対象果実に対して設定距離内に接近した
ことを検出する赤外光利用の近接センサであって、それ
ら両センサ（S₁），（S₂）は、アプローチや果実収穫の
邪魔にならないように、前記バキュームパッド（11）の
内部に設けられている。又、（15）は前記イメージセン
サ（S₁）の撮像処理に同期して前記作業用ハンド（Ｈ）
の前方側を設定光量で照光するための照光装置である。

そして、前記作業用ハンド（Ｈ）を収穫対象果実が位置
する方向に向けた状態で、前記照光装置（15）にて照光
しながら、前記イメージセンサ（S₁）にて撮像処理し、
撮像した画像情報に基づいて、前記作業用ハンド（Ｈ）
のアプローチ方向つまり前記作業用ハンド（Ｈ）を移動
させるための設定軌跡（Ｌ）の方向を決定するようにし
てある。

アプローチ方向を決定した後は、前記作業用ハンド
（Ｈ）が決定されたアプローチ方向に向かって伸縮する
ように、前記各電動モータ（9a），（9b），（9c）を作
動させると共に、前記近接センサ（S₂）が果実への接近
を検出するに伴って、前記バキュームパッド（11）を吸
引作動させて収穫対象果実（Ｆ）を吸着し、そして、前
記捕捉部ケース（13）内に果実（Ｆ）を取り込んだ後、
柄を切断して収穫させることになる。

但し、前記イメージセンサ（S₁）の撮像視野内に複数個
の果実がある場合には、何れの果実から先にアプローチ
させるかを、前記イメージセンサ（S₁）による撮像画像
の明るさ情報に基づいて、アプローチ順序を自動的に決
定させるようにしてある。

前記画像処理について説明を加えれば、第４図に示すよ
うに、前記作業用ハンド（Ｈ）を、予め設定されたアプ
ローチ開始位置にセットした状態で撮像処理し、その撮
像情報を画像処理することにより、前記果実（Ｆ）の色
に対応した特定色領域（F₁）と、その特定色領域（F₁）
内において明るさが設定値以上となる設定領域（F₂）
と、その設定領域（F₂）の重心（F₃）とを抽出するよう
にしてある。そして、前記設定領域（F₂）の大きさが最
も大きいものを抽出して、その重心（F₃）に向けて前記
作業用ハンド（Ｈ）を先にアプローチさせるべく、前記
作業用ハンド（Ｈ）の向きを調節して、前記アーム
（８）を伸長させることになる。

つまり、第５図にも示すように、前記画像情報の明るさ
は、距離に反比例して近いものほど明るくなることか
ら、前記特定色領域（F₁）内において明るさが設定値以
上となる設定領域（F₂）の大きさは、近距離に位置する
ものほど大になることを利用して、手前側に位置する収
穫し易い果実（Ｆ）から先にアプローチさせるようにし
ているのである。

前記特定色領域（F₁）を抽出するための画像処理につい
て説明を加えれば、前記イメージセンサ（S₁）から出力
される画像信号のうちの三原色信号（Ｒ），（Ｇ），
（Ｂ）を用いて、果実（Ｆ）の色成分を含む赤色信号
（Ｒ）からそれ以外の色信号（Ｇ又はＢ）を減算した信
号を設定閾値に基づいて２値化することにより、前記イ
メージセンサ（S₁）にて撮像された原画像（第４図
（イ）参照）から、前記果実（Ｆ）の色に対応する特定
色領域（F₁）を抽出した２値化画像（第４図（ロ）参
照）を得るようにしてある。

そして、前記イメージセンサ（S₁）から出力される画像
信号のうちの輝度信号（Ｙ）の値に基づいて、前記特定
色領域（F₁）内における明るさが設定値以上となる設定
領域（F₂）を抽出すると共に、その設定領域（F₂）の重
心（F₃）が画像上において位置する方向を、前記果実
（Ｆ）の位置情報として求めることになる。（第５図参
照）。

但し、第４図（ハ）にも示すように、一つの特定色領域
（F₁）内に前記設定領域（F₂）の複数個が抽出された場
合には、その設定領域（F₂）の大きさに基づいて、大き
さが大なるものほど先にアプローチさせるようにアプロ
ーチ順序を決定することになる。

次に、前記イメージセンサ（S₁）の撮像情報に基づいて
前記作業用ハンド（Ｈ）を作業対象果実に向けて誘導す
るための制御構成について説明する。

第２図に示すように、前記イメージセンサ（S₁）による
撮像画像情報を画像処理して、収穫対象果実（Ｆ）の位
置する方向を判別すると共に、アプローチ順序を決定す
るための各手段を構成し、且つ、決定されたアプローチ
順序で、前記作業用ハンド（Ｈ）を収穫対象果実（Ｆ）
に向けて誘導するように、前記ブーム昇降用油圧シリン
ダ（５）、前記ブーム旋回用油圧シリンダ（６）、前記
補助ブーム揺動用油圧シリンダ（７）、前記アーム
（８）の駆動用各電動モータ（9a），（9b），（9c）の
夫々の作動を制御するマイクロコンピュータ利用の制御
装置（16）と、その制御装置（16）の指令に基づいて、
前記アーム（８）の駆動用各電動モータ（9a），（9
b），（9c）の夫々を駆動するサーボコントローラ（1
7）とが設けられている。

つまり、前記制御装置（16）を利用して、前記作業用ハ
ンド（Ｈ）を直線状の設定軌跡（Ｌ）に沿って移動させ
るように、前記旋回用電動モータ（9a）、前記揺動用電
動モータ（9b）、及び、前記伸縮用電動モータ（9c）夫
々の単位時間当たりの目標駆動量を設定時間毎に求める
目標駆動量演算手段（100）が構成されることになり、
そして、前記サーボコントローラ（17）が、前記目標駆
動量演算手段（100）にて求められた目標駆動量で前記
揺動用電動モータ（9b）及び伸縮用電動モータ（9c）の
夫々を駆動する駆動手段（101）に対応することにな
る。

尚、前記各電動モータ（9a），（9b），（9c）の夫々に
は、前記基端部ベース（8a）や各アーム（8b），（8c）
の揺動角度を検出するためのエンコーダ（e₀），
（e₁），（e₂）が付設され、前記サーボコントローラ
（17）は、基本的には、それらエンコーダ（e₀），
（e₁），（e₂）の検出情報に基づいて、前記基端部ベー
ス（8a）や各アーム（8b），（8c）の揺動角度
（θ_０），（θ_１），（θ_２）が目標駆動量に対応した
位置となるように、駆動量をフィードバック制御するよ
うになっている。

但し、前記各電動モータ（9a），（9b），（9c）は、駆
動用の供給電圧をPWM変調することにより、駆動量を調
節されるように構成され、そして、各目標駆動量（Duty
₀），（Duty₁），（Duty₂）の夫々を、PWM変調における
デューティ比の値として設定するようにしてある。つま
り、前記サーボコントローラ（17）は、前記目標駆動量
演算手段（100）によって求められる目標駆動量（Dut
y₀），（Duty₁），（Duty₂）となるように供給電圧
（Y_B）をPWM変調して、前記各電動モータ（9a），（9
b），（9c）を駆動することになる。

尚、前記作業用ハンド（Ｈ）の水平方向での向きを変更
するための前記アーム旋回用電動モータ（9a）は、前記
揺動用モータ（9b）及び前記伸縮用電動モータ（9c）の
ように駆動負荷が大きく変化することはなく、又、作業
用ハンド（Ｈ）が収穫対象果実に達するまでに、その向
きを収穫対象果実が位置する方向に合わせることができ
ればよいものである。従って、前記揺動用モータ（9b）
及び前記伸縮用電動モータ（9c）のように高速駆動する
必要がないので、その目標駆動量を求めるための構成に
ついての説明は省略する。

次に、前記揺動用電動モータ（9b）及び前記伸縮用電動
モータ（9c）夫々の目標駆動量（Duty₁），（Duty₂）を
求める目標駆動量演算手段（100）について説明する。

前記各電動モータ（9b），（9c）の目標駆動量、すなわ
ち、供給電圧（V_B）をPWM変調するためのデューティ比
（Duty）は、例えば、前記第２アーム（8c）を駆動する
電動モータ（9c）の目標駆動量（Duty₂）を例に説明す
れば、トルク（τ）、及び、角速度（_２）に基づい
て、下記（ｉ）式から求めることができる。

但し、K_Tはトルク定数、ｒはモータの電機子抵抗、Ｒは
減速比である。

又、前記トルク（τ）、及び、角速度（ｉ）は、前記
第１、第２アーム（8b），（8c）夫々の枢支点にかかる
慣性力（Ｊ）や重力（Ｇ）に基づいて、決定できること
から、前記電動モータ（9c）のトルク（τ）は、下記
（ii）式に示すように書き換えることができる。

τ＝J₂₁・_１＋J₂₂・_２＋G₂＋V₂・_２…… （ii） 但し、V₂は、前記第２アーム（8c）における粘性抵抗係
数である。

ところで、前記作業用ハンド（Ｈ）を高速移動させるた
めには、前記減速比（Ｒ）を小さくすればよいのである
が、前記減速比（Ｒ）を小さくすると、上記（ｉ），
（ii）式からも明らかなように、目標駆動量つまり前記
作業用ハンド（Ｈ）の移動軌跡の誤差に対して前記慣性
力（Ｊ）や重力（Ｇ）の影響が大きくなる。

そこで、詳しくは後述するが、前記伸縮アーム（８）を
実際に駆動しながら、後述の基準値（Ｍ）に基づいて前
記目標駆動量（Duty）を設定時間毎に求める際に、前記
粘性抵抗係数（V₂）を駆動結果に基づいて補正して、設
定軌跡（Ｌ）からのずれが少なくなるようにチューニン
グさせながら前記作業用ハンド（Ｈ）を誘導するように
してある。

一方、前記作業用ハンド（Ｈ）のアプローチ開始位置か
らの移動距離（σ）、及び、仰角（η）は、前記第１、
第２アーム（8b），（8c）夫々の長さ（Ａ）と、前記作
業用ハンド（Ｈ）の長さ（Ｂ）と、前記第１アーム（8
b）の鉛直方向に対する揺動角（θ_１）と、前記第２ア
ーム（8c）の前記第１アーム（8b）に対する揺動角（θ
_２）とから、下記（iv），（ｖ）式に基づいて、夫々求
めることができる。

σ＝2A・sin（θ₂/2）＋Ｂ ……（iv） η＝θ₂/2−θ_１ ……（ｖ） 但し、前記仰角（η）は、その変化が小さいので、上記
（ｖ）式を二回微分することにより、前記加速度
（_１）は、_１＝₂/2とすることができる。

従って、設定時間（Δｔ）後の前記第１、第２アーム
（8a），（8c）の目標揺動角（θｉ）は、角速度（
ｉ）、及び角加速度（）の夫々に基づいて、下記
（vi）式にて求めることができる。

θｉ＝θ_i0＋（ｉ＋_i0）・Δt/2 ……（vi） 従って、前記目標駆動量（Duty₂）に対応する 予測角速度（_２）は、下記（vii）式に基づいて求め
ることができる。

但し、D₂＝G₂−Duty₂ C₂＝V₂＋モータの逆起電力係数₂₀ ＝初期値 である。

つまり、前記目標駆動量（Duty₁），（Duty₂）の夫々
は、アームの現在位置つまり前記第１アーム（8b）及び
前記第２アーム（8c）夫々の現在の揺動角
（θ_{１ （ｎ）}），（θ_{２ （ｎ）}）の値と、設定時間（Δ
ｔ）後の揺動角（θ_{１ （ｎ＋１）}），
（θ_{２ （ｎ＋１）}）との差から、上記（ｉ）乃至（vi
i）式を用いて求めることができるのである。

そして、前記目標駆動量（Duty）に対応する予測角速度
（ｉ）と、前記エンコーダ（e₁），（e₂）の検出情報
に基づいて求められる実際の角速度とを比較して、実際
の角速度が予測角速度（ｉ）よりも速い場合には、前
記粘性抵抗係数（Vi）とモータの逆起電力係数とを加算
した値（Ci）（以下の説明において粘性抵抗情報（Ci）
と呼称する）を設定量（ΔVi）だけ減少し（第８図
（イ）参照）、 実際の角速度が予測角速度（ｉ）よりも遅い場合に
は、前記粘性抵抗情報（Ci）を設定量（ΔCi）だけ増大
させて（第８図（ロ）参照）、実際の角速度が前記予測
角速度（ｉ）となるように補正して、前記設定軌跡
（Ｌ）に対する実際の移動軌跡のずれが少なくなるよう
にしているのである。

ところで、アーム全体を縮めた状態からアームを伸ばし
て前記作業用ハンド（Ｈ）を重力に抗する方向に突出さ
せる場合（前進時）には、前記第２アーム（8c）が前記
第１アーム（8b）の先端部に取り付けられ、且つ、その
第２アーム（8c）を重力に抗する方向に揺動させること
になるために、前記伸縮用電動モータ（9c）に対する負
荷のほうが前記揺動用電動モータ（9b）に対する負荷よ
りも大となり、一方、アーム全体を伸ばした状態からア
ームを縮めて作業用ハンド（Ｈ）を引退させる場合（後
退時）には、前記第１アーム（8b）を重力に抗する方向
に揺動させることになるために、前記揺動用電動モータ
（9b）に対する負荷のほうが前記伸縮用電動モータ（9
b）に対する負荷よりも大となる。

つまり、伸縮作動の間は、何れか一方の電動モータに対
する負荷は、他方の電動モータに対する負荷よりも軽い
状態となることから、その負荷が大なる側の電動モータ
に対する目標駆動量が極力大となるようにして、負荷が
小なる側の電動モータに対する目標駆動量を制御するよ
うにすると、駆動エネルギーを有効利用できるので、全
体としての前記作業用ハンド（Ｈ）の移動速度を高速化
できることになる。

そこで、第３図に示すように、負荷が大なる側の電動モ
ータの目標駆動量の基準値（Ｍ）、すなわち、前記PWM
変調におけるデューティ比の初期値を、最大駆動量から
前記サーボアンプ（18）によるフィードバック制御の制
御マージン（±20％）の半分を減算した90％に初期設定
してアプローチを開始させるようにしてある。

アプローチを開始した後は、設定時間（Δｔ）毎に、負
荷が大なる側の粘性抵抗情報（Ci）を駆動結果に基づい
て補正しながら、負荷が小なる側の電動モータに対する
目標駆動量を前記設定時間（Δｔ）毎のアームの現在位
置に基づいて補正しながら求めさせることになる。つま
り、制御精度が悪化しない範囲で負荷が大なる側の目標
駆動量（Duty）の基準値（Ｍ）を最大に設定して前記伸
縮アーム（８）を駆動し、且つ、実際に作業用ハンド
（Ｈ）を目標にに向けて移動させている間に、前記伸縮
アーム（８）をチューニングするようにしているのであ
る。

そして、収穫作業を行う毎に、つまり、前記作業用ハン
ド（Ｈ）を前記設定軌跡（Ｌ）に沿って移動させる毎
に、前記目標駆動量（Duty）の基準値（Ｍ）を、前記サ
ーボコントローラ（17）によるフィードバック制御の制
御マージンが零となるまで、つまり、前記サーボコント
ローラ（17）によって実際に駆動される駆動量が最大駆
動量となるまで漸次増大させて、前記作業用ハンド
（Ｈ）が前記電動モータ（9b又は9c）を駆動可能な最大
速度で移動するようにするのである。

但し、前記実際の駆動量が最大駆動量となるときには、
前記目標駆動量（Duty）の基準値（Ｍ）は、前記サーボ
コントローラ（17）によるフィードバック制御の制御マ
ージン分だけ大に設定する必要があることから、前記基
準値（Ｍ）として設定される実際の最大値は120％にな
る（第９図（イ）乃至（ニ）参照）。

ところで、ハンド後退時においては、その移動経路の途
中から、前記電動モータ（9b），（9c）に対する負荷の
大小の関係が逆転する状態となる。そこで、詳述はしな
いが、負荷の大小が逆転する時点から、目標駆動量を一
定値に設定する側の電動モータを切り換えるように、前
記目標駆動量（Duty₁），（Duty₂）の夫々を求める毎
に、その大小を比較して、大なる値となる側の駆動量を
大に設定することになる。

尚、ハンド前進時には前記近接センサ（S₂）がON作動す
るに伴って停止指令が与えられることになり、後退時に
は初期位置に復帰するに伴って停止指令が与えられるこ
とになる。そして、停止指令が与えられると、前記目標
駆動量（Duty₁），（Duty₂）を停止用の値に切り換え
て、前記作業用ハンド（Ｈ）を停止させることになる。

又、詳述はしないが、前記停止用の目標駆動量の夫々
は、前記角速度（ｉ）の値を零とし、且つ、前記角加
速度（ｉ）の値を、前記第１、第２アーム（8b），
（8c）の夫々に対する駆動方向が逆転する値（−_i0/
Δｔ）に設定することにより、上記（ｉ）乃至（vii）
式を用いて、前記動作用の目標駆動量（Duty₁），（Dut
y₂）を求める場合と同様にして求めることができる。

〔別実施例〕

上記実施例では、果実（Ｆ）の位置情報として、特定色
領域（F₁）内における明るさが設定値以上となる設定領
域（F₂）の重心（F₃）を用いるようにした場合を例示し
たが、前記特定色領域（F₁）内における明るさが最大と
なる領域や、前記特定色領域（F₁）自体の重心を用いる
こともできる。

又、上記実施例では、本発明を果実収穫用の作業機に適
用した場合を例示したが、本発明は、収穫機以外の各種
作業機にも適用できるものであって、目標位置に向けて
作業用ハンドを直線状の設定軌跡に沿って移動させるよ
うに多関節型アームの伸縮を制御する各種の装置に適用
できる。そして、本発明を実施するに必要となる各部の
具体構成は、各種変更できる。

尚、特許請求の範囲の項に図面との対照を便利にする為
に符号を記すが、該記入により本発明は添付図面の構造
に限定されるものではない。

【図面の簡単な説明】 図面は、本発明にかかる多関節型アームの伸縮制御装置
の実施例を示し、第１図は多関節型アームの概略側面
図、第２図は制御構成のブロック図、第３図は制御作動
のフローチャート、第４図（イ）乃至（ハ）及び第５図
は画像処理の説明図、第６図は作業用ハンドの概略構成
を示す断面図、第７図は作業機の全体側面図、第８図
（イ），（ロ）はチューニングの説明図、第９図（イ）
乃至（ニ）は作業用ハンドの移動速度増大の説明図であ
る。 （Ｈ）……作業用ハンド、（8b）……第１アーム、（8
c）……第２アーム、（9b）……第１アクチュエータ、
（9c）……第２アクチュエータ、（100）……目標駆動
量演算手段、（101）……駆動手段。

Claims (1)

【特許請求の範囲】
1. 【請求項１】第１アクチュータ（9b）によって上下揺動
   駆動自在な第１アーム（8b）が設けられ、先端部に作業
   用ハンド（Ｈ）を備えた第２アーム（8c）が、前記第１
   アーム（8b）の先端部に対して第２アクチュエータ（9
   c）によって上下揺動駆動自在に枢支され、前記作業用
   ハンド（Ｈ）を直線状の設定軌跡に沿って移動させるよ
   うに、前記第１アクチュエータ（9b）及び前記第２アク
   チュエータ（9c）夫々に対する単位時間当たりの目標駆
   動量を設定時間毎に求める目標駆動量演算手段（100）
   と、その目標駆動量演算手段（100）にて求められた目
   標駆動量の情報に基づいて前記第１アクチュエータ（9
   b）及び前記第２アクチュエータ（9c）の夫々を駆動す
   る駆動手段（101）とが設けられた多関節型アームの伸
   縮制御装置であって、前記目標駆動量演算手段（100）
   は、前記第１アクチュエータ（9b）及び前記第２アクチ
   ュエータ（9c）のうち駆動負荷の大きいがわの初期の駆
   動量を最大駆動量より予め設定された低い駆動量を基準
   目標駆動量として、該基準目標駆動量で駆動する駆動負
   荷の大きいがわの実際の駆動量を検出してその検出結果
   に基づいて駆動負荷の小さいがわの単位時間当たりの目
   標駆動量を補正するとともに、駆動負荷の大きいがわに
   おける目標駆動量を求めるための記憶された粘性抵抗情
   報を駆動結果に基づいて設定時間毎に補正するように構
   成され、且つ、前記設定軌跡に沿う移動を行う毎に、前
   記目標駆動量を求めるために記憶された前記基準目標駆
   動量を漸次増大するように構成されている多関節型アー
   ムの伸縮制御装置。