JPH0790478B2 - 収穫作業機の多関節型アームの伸縮制御装置 - Google Patents

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

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

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

尚、前記アーム旋回用電動モータ（9a）は、アプローチ
開始位置において、前記作業用ハンド（Ｈ）の水平方向
での向きを収穫対象果実の位置する方向に向けた後は、
停止されることになり、前記揺動用モータ（9b）及び前
記伸縮用電動モータ（9c）のように、駆動量を制御する
必要がないので、その目標駆動量を求めるための構成に
ついての説明は省略する。

次に、前記揺動用電動モータ（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）の目標揺動角（θｉ）は、角速度（
ｉ）及び角加速度（ｉ）の夫々に基づいて、下記（v
i）式にて求めることができる。

θｉ＝θ_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₂）の夫々
は、アームの現在位置つまり前記第１アーム（8b）及び
前記第２アーム（8c）夫々の現在の揺動角（θ_1(n)），
（θ_2(n)）の値と、設定時間（Δｔ）後の揺動角（θ
_1(n+1)），（θ_2(n+1)）との差から、上記（ｉ）乃至
（vi）式を用いて求めることができるのである。

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

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

そこで、負荷が大なる側の電動モータの目標駆動量、す
なわち、前記PWM変調におけるデューティ比の値を、最
大駆動量に対応する100％の一定値に設定すると共に、
設定時間（Δｔ）毎に、負荷が小なる側の電動モータに
対する目標駆動量を、前記設定時間（Δｔ）毎のアーム
の現在位置に基づいて補正しながら求めるようにしてあ
る。

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

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

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

次に、第３図（イ），（ロ）に示すフローチャートに基
づいて、負荷が大なる側の電動モータの目標駆動量（Du
ty₁又はDuty₂）を最大値（100％）に設定して、負荷が
小なる側の電動モータの目標駆動量を求めるための処理
について説明を加える。

但し、バンド前進時及びバンド後退時の何れにおいても
前記第２アーム（8c）の負荷が前記第１アーム（8b）よ
りも大であると仮定して、その負荷が大となる目標駆動
量（Duty₂）を100％に設定し、そして、前記第１アーム
（8b）の目標駆動量（Duty₁）を設定時間（Δｔ）毎に
求めさせるものとする。

先ず、第３図（イ）に示すフローチャートに基づいて、
ハンド前進時における目標駆動量演算処理について説明
する。

すなわち、前記制御装置（17）は、前記設定時間（Δ
ｔ）毎に、前記サーボコントローラ（18）を介して入力
される前記アーム揺動用モータ（9b）に対するエンコー
ダ（e₁）の検出情報、及び、前記アーム伸縮用モータ
（9c）に対するエンコーダ（e₂）の検出情報の夫々に基
づいて、前記作業用ハンド（Ｈ）の現在位置（ｎ）に対
応する値となる前記アーム（8b），（8c）の揺動角の現
在値（θ_1(n)），（θ_2(n)）の夫々を求め、そして、そ
の値に基づいて慣性力（J_(n)）や重力（G_(n)）の夫々を
求める。

つまり、前記エンコーダ（e₁），（e₂）の検出情報に基
づいて前記アーム（8b），（8c）の揺動角の現在値（θ
_1(n)），（θ_2(n)）を求める処理が、設定軌跡上におけ
る前記作業用ハンド（Ｈ）の位置を検出する位置検出手
段（102）に対応することになる。

次に、上記（iv）式から、次の目標位置における前記第
２アーム（8c）の目標角速度（₂ ^* _(n+1)）及び目標揺
動角（θ₂ ^* _(n+1)）の夫々を求め、その目標揺動角（θ₂
^* _(n+1)）の値から前記第１アーム（8b）の目標揺動角
（θ₁ ^* _(n+1)）を求める。

そして、前記両目標揺動角（θ₁ ^* _(n+1)），
（θ₂ ^* _(n+1)）に基づいて、設定時間（Δｔ）後の慣性
力（（J_(n+1)）及び前記重力（G_(n+1)）の夫々を求め、
それらの値から、次の目標位置（ｎ＋１）における目標
角速度（₂ ^* _(n+2)）及び目標揺動角（θ₂ ^* _(n+2)）の夫
々を求める。

次に、求めた目標揺動角（θ₂ ^* _(n+2)）の値から、前記
第１アーム（8b）の縦軸芯（Ｙ）周りでの目標旋回角
（θ₀ ^* _(n+2)）及び前記第１アーム（8b）の横軸芯周り
での目標揺動角（θ₁ ^* _(n+2)）の夫々を求めて、それら
の値から、前記アーム旋回用電動モータ（9a）の目標駆
動量（Duty₀）と前記アーム揺動用電動モータ（9b）の
目標駆動量（Duty₁）の夫々を求めて、前記アーム伸縮
用電動モータ（9c）の目標駆動量（Duty₂＝100％）と共
に、前記サーボコントローラ（18）に渡し、且つ、次の
目標駆動量演算のために、前記サーボコントローラ（1
8）から次の目標位置における前記揺動角の現在値（θ
_1(n+1)），（θ_2(n+1)）の夫々を受け取った後、目標位
置設定用の変数（ｎ）の値に“1"を加算することにな
る。

ハンド後退時における目標駆動量演算処理について説明
すれば、第３図（ロ）に示すように、上述のハンド前進
時と同様にして、次の目標位置における前記第１アーム
（8b）の目標揺動角（θ₁ ^* _(n+1)）と前記第２アーム（8
c）の目標揺動角（θ₂ ^* _(n+1)）の夫々を求めると共に、
求めた目標揺動角（θ₁ ^* _(n+1)），（θ₂ ^* _(n+1)）の値か
ら、前記慣性力（（J_(n+1)）及び重力（G_(n+1)）の夫々
を求める。

次に、上記（iv）式を用いて、求めた前記慣性力（（J
_(n+1)）及び重力（G_(n+1)）から、前記第２アーム（8
c）の目標角速度（₂ ^* _(n+2)）及び目標揺動角（θ₂ ^*
_(n+2)）の夫々を求めると共に、求めた目標揺動角（θ₂
^* _(n+2)）から、前記縦軸芯（Ｙ）周りでの目標旋回角
（θ₀ ^* _(n+2)）及び前記横軸芯（Ｘ）周りでの目標揺動
角（θ₁ ^* _(n+2)）の夫々を求める。

そして、前記第１アーム（8b）の目標旋回角速度（₀ ^*
_(n+2)）、目標揺動角速度（₁ ^* _(n+2)）、目標揺動角加
速度（₁ ^* _(n+2)）、及び、前記第２アーム（8c）目標
揺動角加速度（₂ ^* _(n+2)）の夫々から、前記各電動モ
ータ（9a），（9b）の目標駆動量（Duty₀），（Duty₁）
の夫々を求めて、前記第２アーム（8c）の目標駆動量
（Duty₂＝100％）と共に前記サーボコントローラ（18）
に渡し、且つ、次の目標駆動量の演算のために、前記サ
ーボコントローラ（18）から前記揺動角の設定時間後に
おける現在値（θ_1(n+1)），（θ_2(n+1)）の夫々を受け
取った後、目標位置設定用の変数（ｎ）の値に“1"を加
算することになる。

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

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

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

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

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

前記アーム（８）の伸長を開始した後は、前記近接セン
サ（S₂）がON作動して、収穫対象果実（Ｆ）に対して設
定距離内に接近したことを検出するまで、前記のよう
に、前記揺動用電動モータ（9b）及び前記伸縮用電動モ
ータ（9c）の単位時間当たりの動作用の目標駆動量（Du
ty₁），（Duty₂）の夫々を、前記設定時間（Δｔ）毎に
求めると共に、その求めた駆動量で前記各電動モータ
（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₂）から、停止用の目標駆動量に切り換え
ることにより、前記アーム（８）の伸縮作動を停止させ
ることになる。

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

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

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

〔別実施例〕

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

又、上記実施例では、本発明を果実収穫用の作業機に適
用した場合を例示したが、本発明を実施するに必要とな
る各部の具体構成は、各種変更できる。

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

【図面の簡単な説明】

図面は本発明にかかる収穫作業機の多関節型アームの伸
縮制御装置の実施例を示し、第１図は多関節型アームの
概略側面図、第２図は制御構成のブロック図、第３図
（イ）はハンド前進時における目標駆動量演算処理のフ
ローチャート、同図（ロ）はハンド後退時における目標
駆動量演算処理のフローチャート、第４図は制御作動の
フローチャート、第５図（イ）乃至（ハ）は画像処理の
説明図、第６図は作業用ハンドの概略構成を示す断面
図、第７図は作業機の全体側面図である。 （Ｈ）……作業用ハンド、（8b）……第１アーム、（8
c）……第２アーム、（9b），（9c）……揺動用アクチ
ュエータ、（100）……目標駆動量演算手段、（101）…
…駆動手段、（102）……位置検出手段。

Claims (1)

【特許請求の範囲】
1. 【請求項１】第１アクチュエータ（9b）によって上下揺
   動駆動自在な第１アーム（8b）が移動体に搭載設置さ
   れ、先端部に作業用ハンド（Ｈ）を備えた第２アーム
   （8c）が、前記第１アーム（8b）の先端部に対して第２
   アクチュエータ（9c）によって上下揺動駆動自在に枢支
   され、前記作業用ハンド（Ｈ）を直線状の設定軌跡に沿
   って移動させるように、前記第１アクチュエータ（9b）
   及び前記第２アクチュエータ（9c）夫々に対する単位時
   間当たりの目標駆動量を設定時間毎に求める目標駆動量
   演算手段（100）と、その目標駆動量演算手段（100）に
   て求められた目標駆動量で前記第１アクチュエータ（9
   b）及び前記第２アクチュエータ（9c）の夫々を駆動す
   る駆動手段（101）とが設けられ、前記目標駆動量演算
   手段（100）は、前記第１アーム（8b）又は前記第２ア
   ーム（8c）のうちの駆動負荷が大なる一方側のアームの
   アクチュエータ（9b又は9c）に対する目標駆動量を所定
   値に設定して、他方側のアームのアクチュエータ（9c又
   は9b）の目標駆動量を求めるように構成されている収穫
   作業機の多関節型アームの伸縮制御装置であって、前記
   設定軌跡上における前記作業用ハンド（Ｈ）の位置を検
   出する位置検出手段（102）が設けられ、前記目標駆動
   量演算手段（100）は、前記第１アーム（8b）又は前記
   第２アーム（8c）のうちの駆動負荷が大なる一方側のア
   ームのアクチュエータ（9b又は9c）に対する目標駆動量
   を最大駆動量に設定して、駆動負荷が小なる他方側のア
   ームのアクチュエータ（9c又は9b）の目標駆動量を、前
   記位置検出手段（102）の検出情報に基づいて補正する
   ように構成されている収穫作業機の多関節型アームの伸
   縮制御装置。