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

Description

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

〔従来の技術〕

上記この種の収穫作業機の多関節型アームの伸縮制御装
置において、アクチュエータの最大駆動力には限度があ
ることから、最大負荷の時にアクチュエータの駆動力が
最大駆動力以下となるように、目標駆動量として設定可
能な最大値は、最大駆動力によって制限されることにな
る。

作業用ハンドの移動速度を高速化するためには、アクチ
ュエータの最大駆動力が大なる大型のアクチュエータを
用いればよいが、装置が高価になる不利がある。

ところで、この種の収穫作業機の多関節型アームの伸縮
制御装置においては、作業用ハンドの移動中における速
度変化や単位時間当たりの移動量の変動はあまり問題に
ならないことが多い。

そこで、従来では、負荷が大なる側のアクチュエータの
駆動量を極力大に設定して、負荷が小なる側のアクチュ
エータの駆動量を制御することにより、設定軌跡に対す
るずれを抑制しながら、限られた最大駆動量を有効利用
して、極力高速移動させることができるようにしていた
（本出願人が先に提案した特願昭63−222064号参照）。

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

上記従来構成では、負荷が小なる側の目標駆動量は、駆
動可能な最大駆動量から設定軌跡からのずれを修正可能
なフィードバック制御のための制御マージンを除いた値
以下に設定することになり、第１、第２アクチュエータ
の目標駆動量の両方を、駆動可能な最大駆動量には設定
できないものであり、改善の余地があった。

ところで、収穫作業のための撮像を行ってその撮像情報
に基づいて収穫対象物を特定して、その収穫対象物を収
穫するアプローチ経路としての設定軌跡を算出して、収
穫対象物へ精度良く作業用ハンドを移動させる必要性か
ら、そのアプローチ開始する位置の近傍に作業対象があ
るように設定することは少なく、従って、移動開始時点
から設定軌跡からのずれがない状態で誘導する必要はな
いものである。

従って、アプローチを開始する位置の近傍においては、
第１、第２アクチュエータの両方を駆動可能な最大駆動
量で駆動しても、作業対象が位置する箇所に達するまで
に、設定軌跡に対するずれを修正できれば、作業用ハン
ドの移動速度を更に高速化できることになる。

特に、移動開始時点における駆動量を大にできれば、初
期加速力が大となるので、高速化に有利となる。

本発明は、上記実情に鑑みてなされたものであって、そ
の目的は、作業用ハンドの移動速度の高速化を図ること
のできる収穫作業機の多関節型アームの伸縮制御装置を
提供することにある。

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

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

すなわち、前記目標駆動量演算手段は、前記作業用ハン
ドが収穫作業のアプローチ開始位置から設定距離を移動
する間は、前記第１アクチュエータ及び前記第２アクチ
ュエータの夫々に対する目標駆動量を駆動可能な最大駆
動量に設定するように構成されている点にある。

〔作 用〕

前述のように、作業用ハンドの収穫作業のためのアプロ
ーチ開始時点では、作業用ハンドのの移動軌跡が設定軌
跡から多少ずれても問題にならないことから、アプロー
チ開始位置から設定距離を移動する間は、第１アクチュ
エータ及び第２アクチュエータの夫々に対する目標駆動
量を駆動可能な最大駆動量に設定することにより、移動
開始時における加速力を最大にするのである。

〔発明の効果〕

従って、アプローチ開始時点における加速力を最大にで
きるので、作業用ハンドが収穫対象物に達するまでに要
する時間を短縮できるものとなる。もって、合理的な装
置改造によって、アクチュエータの駆動力を最大限に有
効利用して、収穫作業に供せられる作業用ハンドの移動
速度を高速化でき、収穫作業における作業能率を高くで
きるに至った。

〔実施例〕

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

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

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

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

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

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

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

一方、前記作業用ハンド（Ｈ）のアプローチ開始位置か
らの移動距離（σ）、及び、仰角（η）は、前記第１、
第２アーム（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）夫々の現在の揺動角（θ_1(n)），
（θ_2(n)）の値と、設定時間（Δｔ）後の揺動角（θ
_1(n+1)），（θ_2(n+1)）との差から、上記（ｉ）乃至
（vii）式を用いて求めることができるのである。

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

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

そこで、負荷が大なる側の電動モータの目標駆動量、す
なわち、前記PWM変調におけるデューティ比を、駆動可
能な最大駆動量に対応する最大値に設定して、負荷が小
なる側の電動モータの目標駆動量を駆動結果に基づいて
制御するようにしてある。

但し、前記作業用ハンド（Ｈ）がその初期位置から設定
距離（200mm）を移動する間には作業対象が無いので、
その間における移動軌跡は、前記設定軌跡（Ｌ）から多
少ずれても誘導精度に影響しないことになる。

そこで、前記作業用ハンド（Ｈ）がその初期位置から設
定距離（200mm）を移動する間は、前記両電動モータ（9
b），（9c）の目標駆動量（Duty₁），（Duty₂）の夫々
を、駆動可能な最大値に設定して、加速度を最大にでき
るようにしてある。

尚、実際には、前記サーボコントローラ（17）は、与え
られた目標駆動量に対して、前記各エンコーダ（e₁），
（e₂）の情報に基づいて判別される位置情報から、設定
軌跡（Ｌ）に対するずれを修正するためのフィードバッ
ク制御を行うことになるため、実際に各電動モータ（9
b），（9c）を駆動可能な最大駆動量（100％）で駆動す
るためには、前記目標駆動量（Duty₁），（Duty₂）の最
大値は、前記最大駆動量（100％）に前記フィードバッ
ク制御の制御マージンを加えた値（例えば、120％）に
設定することになる。

そして、前記両電動モータ（9b），（9c）を駆動可能な
最大駆動量で駆動する場合において、特に、作業用ハン
ド（Ｈ）の前進時には、前記第１アーム（8b）が重力の
作用する下向きに揺動駆動されるので、前記第１アーム
（8b）に作用する重力をも有効利用して高速駆動できる
ことになり、前記作業用ハンド（Ｈ）の移動速度の高速
化に有利となる（第７図参照）。

尚、前記作業用ハンド（Ｈ）がその初期位置から前記設
定距離（200mm）を移動した後は、前述の如く、負荷が
大なる側の目標駆動量を最大値に設定し、且つ、設定時
間（Δｔ）毎に、負荷が小なる側の電動モータに対する
目標駆動量を前記設定時間（Δｔ）毎にアームの現在位
置に基づいて補正しながら求めさせることになる。

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

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

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

ところで、前記目標駆動量（Duty）を求めるためには、
前記エンコーダ（e₀），（e₁），（e₂）の情報に基づい
て各アームの揺動角（θｉ）や重力（Gi）の値の夫々
を、例えば、下記（viii），（ix）式に基づいて算出す
る必要がある。

θｉ＝Ki×CNTi ……（viii） Gi＝Mi×ｇ×sin（θｉ） ……（ix） 但し、Kiは定数、Miは各アームの質量、ｇは重力であ
る。

しかしながら、前記各エンコーダから出力される情報
は、通常は実数値である。従って、前記重力（Gi）の値
を求めるためには、三角関数を含む演算を行うことにな
り、演算時間が長くなって、前記目標駆動量（Duty）の
演算間隔となる前記設定時間（Δｔ）の間隔が長くなる
虞れがある。

前記設定時間（Δｔ）の間隔が長くなると、前記伸縮ア
ーム（８）の作動を実時間的な間隔で制御できなくなっ
て、制御精度が低下する虞れがある。

高速演算させるためには、例えば、専用の数値演算用の
プロセッサ等を搭載することが考えられるが、装置構成
が複雑高価になる不利がある。

そこで、詳述はしないが、予め、前記各エンコーダの出
力値（CNTi）に対応する三角関数の値（sin（θｉ））
を求めてテーブル化して、前記制御装置（16）に記憶さ
せ、そして、そのテーブルの先頭アドレス値と前記各エ
ンコーダの出力値（CNTi）の1/2ⁿの値を加算した値をテ
ーブル読み出し用のアドレス値として用いて、前記各エ
ンコーダの出力値（CNTi）から前記三角関数の値（sin
（θｉ））を直接読み出せるようにしてある。

〔別実施例〕

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

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

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

【図面の簡単な説明】

図面は本発明にかかる収穫作業機の各関節型アームの伸
縮制御装置の概略側面図、第２図は制御構成のブロック
図、第３図（イ）乃至（ハ）及び第４図は画像処理の説
明図、第５図は作業用ハンドの概略構成を示す断面図、
第６図は作業機の全体側面図、第７図は作業用ハンドの
移動軌跡の説明図である。 （Ｈ）……作業用ハンド、（8b）……第１アーム、（8
c）……第２アーム、（9b）……第１アクチュエータ、
（9c）……第２アクチュエータ、（100）……目標駆動
量演算手段、（101）……駆動手段、（S₁）……撮像手
段。

Claims (1)

【特許請求の範囲】
1. 【請求項１】第１アクチュエータ（9b）によって上下揺
   動駆動自在な第１アーム（8b）が設けられ、先端部に作
   業用ハンド（Ｈ）を備えた第２アーム（8c）が、前記第
   １アーム（8b）の先端部に対して第２アクチュエータ
   （9c）によって上下揺動駆動自在に枢支されるととも
   に、収穫対象物を撮像する撮像手段（S₁）を備え、この
   撮像手段（S₁）で撮像した後その撮像情報に基づいて算
   出された直線状の設定軌跡に沿って前記収穫対象物に向
   けてアプローチするために前記作業用ハンド（Ｈ）が移
   動するように、前記第１アクチュエータ（9b）及び前記
   第２アクチュエータ（9c）夫々に対する単位時間当たり
   の目標駆動量を設定時間毎に求める目標駆動量演算手段
   （100）と、前記目標駆動量に基づいて前記第１アクチ
   ュエータ（9b）及び前記第２アクチュエータ（9c）の夫
   々を駆動する駆動手段（101）とが設けられた収穫作業
   機の多関節型アームの伸縮制御装置であって、前記目標
   駆動量演算手段（100）は、前記作業用ハンド（Ｈ）が
   収穫作業のアプローチ開始位置から設定距離を移動する
   間は、前記第１アクチュエータ（9b）及び前記第２アク
   チュエータ（9c）の夫々に対する目標駆動量を駆動可能
   な最大駆動量に設定するように構成されている収穫作業
   機の多関節型アームの伸縮制御装置。