JPH0796191B2

JPH0796191B2 - 多関節型アームの伸縮制御装置

Info

Publication number: JPH0796191B2
Application number: JP18258288A
Authority: JP
Inventors: 上田　　吉弘; 正昭西中
Original assignee: Kubota Corp
Current assignee: Kubota Corp
Priority date: 1988-07-21
Filing date: 1988-07-21
Publication date: 1995-10-18
Anticipated expiration: 2010-10-18
Also published as: JPH0236087A

Description

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

〔従来の技術〕

上記この種の多関節型アームの伸縮制御装置において、
従来では、停止指令が与えられるに伴って、揺動用アク
チュエータに対する目標駆動量を、それまでの動作用か
ら停止用に切り換えて演算させるようにしていた。

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

しかしながら、アクチュエータの目標駆動量は、設定時
間毎に演算されるように構成されていることから、従来
のように、停止指令が与えられてから停止用の目標駆動
量を演算させると、アクチュエータが実際に停止用の目
標駆動量で駆動される状態となるまでには、時間遅れが
生じるものとなり、作業用ハンドが設定軌跡上において
実際に停止するまでに目標停止地点からオーバーシュー
トする状態となって、停止指令が与えられた時点で迅速
に停止させることが困難であった。

本発明は、上記実情に鑑みてなされたものであって、そ
の目的は、停止指令が与えられてから作業用ハンドが停
止するまでの時間遅れを短縮できるようにする点にあ
る。

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

本発明の特徴は、冒頭に記したように、先端部に作業用
ハンドを備えた第２アームを、第１アームの先端部に対
して上下揺動自在に枢支し、起動指令によって、作業用
ハンドを直線状の設定軌跡に沿って移動開始させ、且
つ、停止指令によって、作業用ハンドの移動を停止させ
るように、第１アーム及び第２アーム夫々の揺動用アク
チュエータに対する単位時間当たりの目標駆動量を設定
時間毎に求める目標駆動量演算手段と、その目標駆動量
演算手段にて求められた目標駆動量で前記揺動用アクチ
ュエータの夫々を駆動する駆動手段とが設けられている
ものにおいて、前記目標駆動量演算手段は、前記目標駆
動量の演算とは別に、前記停止指令が与えられた時点で
前記作業用ハンドの移動を停止させるため前記揺動用ア
クチュエータ夫々に出力されるべき停止用駆動量の演算
を前記駆動手段への出力前に行うと共に、前記停止指令
が与えられるに伴って、前記目標駆動量に代えて前記停
止用駆動量を前記駆動手段に出力する制御動作を行うよ
うに構成してある点にあり、その作用、及び、効果は次
の通りである。

〔作用〕

つまり、アームの作動時には、この作動を行わせる目標
駆動量と、このアームを現在位置で停止させるための停
止用の目標駆動量とを、目標駆動量演算手段を予め演算
しておき、アームの作動を継続する場合には作動を行わ
せる目標駆動量を駆動手段に出力し、停止指令が与えら
れた場合には、前述のようにこの停止指令が与えられる
前に、先に求めたおいた停止用の目標駆動量を目標駆動
量演算手段が駆動手段に出力する。したがって、従来の
ように停止指令が与えられてから停止用の目標駆動量の
演算を開始するものと比較して、より短時間でアームを
停止させ得るものとなる。

〔発明の効果〕

従って、停止指令が与えられてから作業用ハンドが実際
に停止作動を受けるまでの時間遅れを短縮できて、オー
バーシュートの発生を抑止し、作業用ハンドを目標停止
地点に迅速に停止させ易い装置を得られた。

〔実施例〕

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

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

尚、図中、（５）はブーム昇降用油圧シリンダ、（６）
はブーム旋回用油圧シリンダ、（７）は補助ブーム揺動
用電動モータである。

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

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

そして、側面視において前記第１アーム（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）は、蛇腹式のホース（15）（第８図参照）に
て、前記車体（Ｖ）側に搭載された吸引ポンプ（図示せ
ず）に配管接続されている。

前記捕捉部ケース（13）は、前記通気管（12）に外嵌さ
れる支持部材（13a）に固着された上部ケース（13b）
と、前記支持部材（13a）に対して縦軸芯周りに揺動自
在に枢着された下部ケース（13c）とに２分割して形成
してある。

つまり、前記下部ケース（13c）を、前記通気管（12）
に対して横方向に揺動させることによって、前記捕捉部
ケース（13）内に保持される柄を切断された果実（Ｆ）
を、外部に排出できるようにしているのである。

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

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

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

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

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

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

次に、前記イメージセンサ（S₁）による撮像画像情報に
基づいて、果実（Ｆ）の位置する方向つまりアプローチ
方向を検出すると共に、アプローチ順序を決定するため
の制御構成について説明する。

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

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

尚、詳述はしないが、前記各電動モータ（9a），（9
b），（9c）の夫々にはエンコーダが付設され、前記サ
ーボコントローラ（18）が、それらエンコーダの検出情
報に基づいて、駆動量をフィードバック制御するように
なっている。

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

尚、前記作業用ハンド（Ｈ）の水平方向での向きは、前
記アーム旋回用電動モータ（9a）によって前記アーム
（８）の伸縮に変更される。但し、前記アーム旋回用電
動モータ（9a）は前記アーム（８）を前記縦軸芯（Ｙ）
周りに旋回させるだけであり、それに対する負荷変動
は、前記揺動用電動モータ（9b）や前記伸縮用電動モー
タ（9c）より少ないものである。従って、前記揺動用電
動モータ（9b）や前記伸縮用電動モータ（9c）のよう
に、その駆動速度が作業能率に大きく影響するとはな
く、前記アーム旋回用電動モータ（9a）の目標量を求め
るための構成についての説明は省略する。

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

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

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

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

Duty₁＝J₁₁・_１＋J₁₂・_２＋C₁・_１＋G₁ ……（i
i） Duty₂＝J₂₁・_１＋J₂₂・_２＋C₂・_２＋G₂ ……（ii
i）但し、_ｉは加速度、_ｉは角速度、Ciは粘性抵抗とモ
ータの逆起電力係数とを加算した値である。

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

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

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

θｉ＝θ_i0＋（ｉ＋_i0）・Δt/2 ……（vi）但し、ｉ＝（_i0＋D/C）exp（−ＣΔt/J）−D/C ｉ＝−C/J・（_i0＋D/C）exp（−Δt/J）Ｄ＝Gi−Duty_i Ｃ＝Ci Ｊ＝J_i1/2＋J_i2 である。

尚、_i0は初期値である。

つまり、前記目標駆動量（Duty₁），（Duty₂）の夫々
は、前記設定時間（Δｔ）の前後における目標位置の差
分に基づいて、上記（ｉ）乃至（vi）式を用いて求める
ことができるのである。

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

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

そこで、第２図（イ），（ロ）に示すように、負荷が大
なる側の電動モータの目標駆動量、すなわち、前記PWM
変調におけるデェーティ比の値を、90％の一定値に設定
すると共に、負荷が小なる側の電動モータに対する目標
駆動量を、前記（ｉ）乃至（vi）式に基づいて、設定時
間（Δｔ）毎に求めるようにしてある。

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

尚、負荷が大なる側の目標駆動量を駆動可能な最大値
（100％）ではなく、90％に設定しているのは、前記サ
ーボコントローラ（18）によるフィードバック制御のた
めの制御マージンを確保するためである。

そして、第３図に示すように、停止指令が与えられるに
伴って、前記作業用ハンド（Ｈ）が迅速に停止できるよ
うにするために、動作用の目標移動量を求める時に、現
在位置において急停止させるための停止用の目標駆動量
（Dutys₁），（Dutys₂）の夫々を、同時に求めさせるよ
うにしてある。

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

次に、第５図に示すフローチャートに基づいて、前記制
御装置（17）の動作を説明しながら、前記作業用ハンド
（Ｈ）を収穫対象果実の位置する方向に誘導するための
制御構成について説明する。

先ず、前記作業用ハンド（Ｈ）を、予め設定されたアプ
ローチ開始位置にセットした状態で撮像処理し、その撮
像情報を画像処理することにより、前記果実（Ｆ）に対
応した特定色領域（F₁）と、その特定色領域（F₁）内に
おいて明るさが設定値以上となる設定領域（F₂）と、そ
の設定領域（F₂）の重心（F₃）とを求める（第６図参
照）。

そして、前記設定領域（F₂）の大きさに基づいて、大き
さが最も大きいものを抽出して、その重心（F₃）に向け
て前記作業用ハンド（Ｈ）をアプローチさせるべく、前
記作業用ハンド（Ｈ）の向きを調節して、前記アーム
（８）を伸長させることになる。

説明を加えれば、前記画像情報の明るさは、距離に反比
例して近いものほど明るくなることから、前記特定色領
域（F₁）内において明るさが設定値以上となる設定領域
（F₂）の大きさは、近距離に位置するものほど大になる
のである。

つまり、手前側に位置する収穫し易い果実（Ｆ）から先
にアプローチさせるようにしているのである。

前記アーム（８）の伸長を開始した後は、前記近接セン
サ（S₂）がON作動して、収穫対象果実（Ｆ）に対して設
定距離内に接近したことを検出するまで、前述の如く、
前記揺動用電動モータ（9b）及び前記伸縮用電動モータ
（9c）の単位時間当たりの動作用の目標駆動量（Dut
y₁），（Duty₂）の夫々と、前記停止用の目標駆動量（D
utys₁），（Dutys₂）の夫々とを、前記設定時間（Δ
ｔ）毎に求めると共に、その求めた駆動量で前記各電動
モータ（9b），（9c）の夫々を駆動するように、前記サ
ーボコントローラ（18）に対して指令を与えて、前記作
業用ハンド（Ｈ）を収穫対象果実に向けて直線移動させ
ることになる。

前記近接センサ（S₂）がON作動して収穫対象果実に対し
て設定距離内に接近したこと検出するに伴って、前記バ
キュームパッド（10）の吸引作動を開始して、収穫作動
させることになる。

但し、前記アーム（８）を予め設定された設定距離に達
するまで伸長させても、前記近接センサ（S₂）がON作動
しない場合には、収穫不能であると判断して、前記アー
ム（８）を縮めてアプローチ開始位置まで戻すことにな
る。

収穫作動を開始した後は、前記作業用ハンド（Ｈ）に付
設された各種センサの検出情報に基づいて、収穫を失敗
したか否かを判別し、収穫失敗である場合には、前記作
業用ハンド（Ｈ）を、前記近接センサ（S₂）がON作動し
た位置から更に設定距離を後退させた状態で、再撮像処
理すると共に、その撮像情報を画像処理して、前記作業
用ハンド（Ｈ）のアプローチ方向を再決定して、前記ア
ーム（８）の伸縮方向を修正させた後、再アプローチさ
せるようにしてある。

ところで、再撮像処理した画像情報に基づいて再アプロ
ーチさせても、前記アーム（８）を予め設定された設定
距離を伸長させる間に、前記近接センサ（S₂）がON作動
しない場合には、収穫不能であると判断して、アプロー
チ開始位置まで戻すことになる。

適正通りに果実（Ｆ）を収穫できた場合には、アプロー
チ開始位置まで戻して、収穫した果実を排出させること
になる。

収穫した果実を排出した後は、前記アプローチ開始位置
において最初に撮像した画像情報から抽出した特定色領
域（F₁）の個数等に基づいて、作業終了か否かを判別し
て、作業終了でない場合には、前記作業用ハンド（Ｈ）
をアプローチ開始位置にセットして撮像させる処理以降
の各処理を繰り返させることになる。

尚、詳述はしないが、再アプローチさせた場合において
も収穫を失敗する場合があるが、その場合には、再撮像
処理を行うことなく、アプローチ開始位置まで戻すこと
になる。

つまり、前記アーム（８）の伸長距離が設定距離に達す
る間に前記近接センサ（S₂）がON作動した場合や、前記
アーム（８）を設定距離に達するまで伸長しても前記近
接センサ（S₂）がON作動しない場合や、収穫失敗を検出
した場合等に、前記制御装置（17）から前記サーボコン
トローラ（18）に対して停止指令が与えられることにな
る。

つまり、前記サーボコントローラ（18）は、停止指令が
与えられるに伴って、前記各電動モータ（9b），（9c）
に対する駆動量を、前記動作用の目標駆動量（Dut
y₁），（Duty₂）から、前記停止用の目標駆動量（Dutys
₁），（Dutys₂）に切り換えることにより、前記アーム
（８）の伸縮作動を停止させることになる。

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

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

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

〔別実施例〕

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

又、上記実施例では、負荷が大なる側のアクチュエータ
の目標駆動量を一定値に設定して、負荷が小なる側のア
クチュエータの目標駆動量のみを求めさせるようにした
場合を例示したが、両アクチュエータ夫々の目標駆動量
を求めるようにしてもよい。

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

ちなみに、揺動用アクチュエータ夫々の目標駆動量を、
上記実施例に示すように、第１、第２アーム夫々の揺動
角を制御パラメータとして求めるようにする他、例え
ば、作業用ハンドの設定軌跡上における設定時間毎の目
標位置を等間隔に設定して、作業用ハンドを等速度移動
させるようにしてもよい。

そして、本発明を実施するに必要となる各部の具体構成
は、各種変更できる。

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

【図面の簡単な説明】

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

Claims

【特許請求の範囲】

【請求項１】上下揺動自在な第１アーム（8b）が設けら
れ、先端部に作業用ハンド（Ｈ）を備えた第２アーム
（8c）が、前記第１アーム（8b）の先端部に対して上下
揺動自在に枢支され、起動指令が与えられるに伴って、
前記作業用ハンド（Ｈ）を直線状の設定軌跡に沿って移
動開始させ、且つ、停止指令が与えられるに伴って、前
記作業用ハンド（Ｈ）の移動を停止させるように、前記
第１アーム（8b）及び前記第２アーム（8c）夫々の揺動
用アクチュエータ（9b），（9c）に対する単位時間当た
りの目標駆動量を設定時間毎に求める目標駆動量演算手
段（100）と、その目標駆動量演算手段（100）にて求め
られた目標駆動量で前記揺動用アクチュエータ（9b），
（9c）の夫々を駆動する駆動手段（101）とが設けられ
ている多関節型アームの伸縮制御装置であって、前記目標駆動量演算手段（100）は、前記目標駆動量の
演算とは別に、前記停止指令が与えられた時点で前記作
業用ハンド（Ｈ）の移動を停止させるため前記揺動用ア
クチュエータ（9b），（9c）夫々に出力されるべき停止
用駆動量の演算を前記駆動手段（101）への出力前に行
うと共に、前記停止指令が与えられるに伴って、前記目
標駆動量に代えて前記停止用駆動量を前記駆動手段（10
1）に出力する制御動作を行うように構成してある多関
節型アームの伸縮制御装置。