JPH0825258A - 荷役機械の制御方法 - Google Patents

Abstract

(57)【要約】 【目的】 オペレータが自由な意思でアクチュエータに
速度指令又は位置指令を与える場合に、加加速度又は加
速度、定加速又は定速、減加速度又は減速度の３領域を
設け、現在の速度指令又は位置指令と前回のサンプリン
グ時のアクチュエータの速度又は位置とを比較して、こ
の３領域のいずれかを選択して最適の加加速度又は加速
度、減加速度又は減速度を決めてアクチュエータを自動
的に制御する。 【構成】 速度指令で制御するときに、最適の加速度と
最適の加加速度（ジャークと称する）を定め、現在の速
度と指令の速度との偏差値とを組合せて、加加速、定
速、減加速の３領域の内の１つを選択する方法を定め、
それにより荷役機械の速度制御を行う。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、荷重を吊り下げてオペ
レータの操作で移動させる荷役機械に係り、特に吊り上
げアームの剛牲が十分に高くないため急激に上下させた
場合に振動を起こす可能性のある荷役機械の制御方法に
関する。

【０００２】

【従来の技術】ロボット工学においては、マニピュレー
タを高速にしかも滑らかに動かすために多くの提案がな
され、実施されている。この最も基本的な考え方は、産
業用ロボット・ハンドブック（株式会社日本産業用ロボ
ット工業界１９９３年版Ｐ．１４４〜１４５）に示され
ている。最も簡単な定速制御では、動作の立ち上がり時
（荷重の移動開始時）と停止時（目的地まで移動して停
止した時）に負荷の慣性による行き過ぎと衝撃が起こ
る。そこで、一般には、特定の地点から目的の地点まで
移動する際の動作の立ち上がり時から停止時の全域に渡
って加速、定速、減速の３つの制御領域を設け、速度を
スムーズに変える方法が採られる。しかし、このように
３つの制御領域を設けて荷重の移動制御を行う場合は、
加重移動動作の軌道が事前に明らかになっている場合に
有効である。

【０００３】図８に図示されるマニピュレータは、オペ
レータの自由な意思で荷役機械の荷役アーム２に取り付
けた力センサ４に力を加えてアクチュエータ７に速度指
令を与え吊り荷９を上下動させる装置である。ところ
が、図８に図示のマニピュレータのアーム２が長いため
剛牲が低く、力センサ４に急激な力を加えてアクチュエ
ータ７によって吊り荷９を急激に操作（上下動）しよう
とすると、吊り荷９が上下に激しく揺れて、アクチュエ
ータ７による上下動操作に支障が起る。その後急激に加
速すると、急激な速度指令の増加に対して、荷役機械の
荷役アーム２が加速する前までに少し時間があり、オペ
レータが力センサ４を取り付けた荷役アーム２の動きに
追従できず相対的に急激な減速操作を行うことになり、
荷役機械の荷役アーム２を減速させて振動させる結果と
なる。

【０００４】次に、マニピュレータと同じく、負荷の慣
性の影響で荷役アーム２自身に振動が生じることがあげ
られる。１９９３年版Ｐ．１４４〜Ｐ．１４５のマニピ
ュレータの制御の応用で、予め制御動作の速度が定まっ
ていれば、予め最適の加加速度、減加速度を決めて、そ
れに基づいて加加速、定加速、減加速の３つの制御領域
を設けて加速度をスムーズに変化させる方法を採ること
ができる。しかし、オペレータが自由に操作する場合に
は、制御動作の速度が予め定まっていないので３つの制
御領域を設けて制御することができない。

【０００５】力センサ４に力を加えアクチュエータ７に
速度指令を与えて吊り荷９を上下動させる装置で、オペ
レータが自由に操作する場合、力センサ４からの速度指
令によって動作するアクチュエータ７の動作の立ち上が
り時と停止時に発生する負荷の慣性による行き過ぎと衝
撃をなくすには、オペレータが力センサ４に加える操作
力をゆっくりと、すなわちアクチュエータ７に与える速
度指令をゆっくりとさせて低加速の制御をさせればよ
い。しかし、オペレータが力センサ４を介してアクチュ
エータ７にゆっくりとした速度指令を与えて吊り荷９の
上下動を低加速させるにはオペレータにそれ相応の技量
を必要とする上、オペレータは逆に急激な動作を要求す
ることが多い。

【０００６】

【発明が解決しようとする課題】従来のマニピュレータ
にあっては、オペレータが自由な操作で力センサ４に急
激な力を加えてアクチュエータ７に急激な速度指令を発
した場合、アクチュエータ７の動作によって荷役アーム
２が動作するが、アクチュエータ７は急激な速度指令に
追従しようと荷役アーム２を急激に動作させるため、負
荷の慣性の影響で荷役アーム２自身に振動が生じてしま
うという問題点を有している。

【０００７】本発明の目的は、オペレータが自由な意思
でアクチュエータに速度指令又は位置指令を与える場合
に、加加速度又は加速度、定加速又は定速、減加速度又
は減速度の３領域を設け、現在の速度指令又は位置指令
と前回のサンプリング時のアクチュエータの速度又は位
置とを比較して、この３領域のいずれかを選択して最適
の加加速度又は加速度、減加速度又は減速度を決めてア
クチュエータを自動的に制御することにある。

【０００８】

【課題を解決するための手段】本発明は、速度指令で制
御するときに、最適の加速度と最適の加加速度（ジャー
クと称する）を定め、現在の速度と指令の速度との偏差
値とを組合せて、加加速、定速、減加速の３領域の内の
１つを選択する方法を定め、それにより荷役機械の速度
制御を行うものである。このように構成することで、目
標の速度に達する以前に減加速の状態にすることがで
き、その直後に目標の速度が急激に下げられても荷役機
械に衝撃を与えることなくスムーズに減速状態へと移行
することができる。

【０００９】請求項１記載の発明は、オペレータの操作
に基づいて吊り荷を所定の位置に移動することを目的と
した荷役機械において、上記オペレータの操作に基づく
指令値と荷役機械の現在値との差を求め、該差の時間に
関する正の二次微分値と負の二次微分値と零の微分値の
３つのうち１つを選択してアクチュエータの制御方法を
切換えるようにしたものである。

【００１０】請求項２記載の発明は、オペレータの操作
によって長尺アーム先端に装着した吊り荷を所定の位置
に移動することを目的とした荷役機械において、オペレ
ータの操作によって生ずる長尺アームの速度指令値と該
荷役機械の長尺アームの現在の速度との差を求め、該差
の値に基づいて正のジャーク値と、負のジャーク値と、
零の加速度値及び又はジャーク値の３つの値のうちの１
つを選択してアクチュエータの制御方法を切換えるよう
にしたものである。

【００１１】請求項３記載の発明は、オペレータの操作
によって長尺アーム先端に装着した吊り荷を所定の位置
に移動することを目的とした荷役機械において、オペレ
ータの操作によって生ずる長尺アームの位置指令値と該
荷役機械の長尺アームの現在の位置との差を求め、該差
の値に基づいて正の加速度値と、負の加速度値と、零の
加速度値との３つの値のうちの１つを選択してアクチュ
エータの制御方法を切換えるようにしたものである。

【００１２】請求項４記載の発明は、オペレータの操作
によって長尺アーム先端に装着した吊り荷を所定の位置
に移動することを目的とした荷役機械において、オペレ
ータの操作によって生ずる指令値と該荷役機械の現在値
との差を求め、該差の時間に関する正の二次微分値と、
負の二次微分値と、零の二次微分値及び又は零の一次微
分値との３つのうちの値のうちの１つを選択してアクチ
ュエータの制御方法を切換えるようにしたものである。

【００１３】請求項５記載の発明は、上記正の二次微分
値及び負の二次微分値を、それぞれ誤差の関数としたも
のである。

【００１４】

【作用】請求項１記載の発明の場合、オペレータの操作
に基づいて吊り荷を所定の位置に移動することを目的と
した荷役機械におけるオペレータの操作に基づく指令値
と荷役機械の現在値との差を求め、この差の時間に関す
る正の二次微分値と負の二次微分値と零の微分値の３つ
のうち１つを選択してアクチュエータの制御方法を切換
えることにより、オペレータが自由な意思でアクチュエ
ータに速度指令又は位置指令を与える場合に、現在の速
度指令又は位置指令と前回のサンプリング時のアクチュ
エータの速度又は位置とを比較して、加加速度又は加速
度、定加速又は定速、減加速度又は減速度の３領域のい
ずれかを選択して最適の加加速度又は加速度、減加速度
又は減速度を決めてアクチュエータを自動的に制御す
る。

【００１５】請求項２記載の発明の場合、オペレータの
操作によって長尺アーム先端に装着した吊り荷を所定の
位置に移動することを目的とした荷役機械におけるオペ
レータの操作によって生ずる長尺アームの速度指令値と
該荷役機械の長尺アームの現在の速度との差を求め、こ
の差の値に基づいて正のジャーク値と、負のジャーク値
と、零の加速度値及び又はジャーク値の３つの値のうち
の１つを選択してアクチュエータの制御方法を切換える
ことにより、オペレータが自由な意思でアクチュエータ
に速度指令を与える場合に、現在の速度指令と前回のサ
ンプリング時のアクチュエータの速度とを比較して、加
加速度、定加速又は定速、減加速度の３領域のいずれか
を選択して最適の加加速度、減加速度を決めてアクチュ
エータを自動的に制御できる。

【００１６】請求項３記載の発明の場合、オペレータの
操作によって長尺アーム先端に装着した吊り荷を所定の
位置に移動することを目的とした荷役機械におけるオペ
レータの操作によって生ずる長尺アームの位置指令値と
該荷役機械の長尺アームの現在の位置との差を求め、こ
の差の値に基づいて正の加速度値と、負の加速度値と、
零の加速度値又は零の速度値との３つの値のうちの１つ
を選択してアクチュエータの制御方法を切換えることに
より、オペレータが自由な意思でアクチュエータに位置
指令を与える場合に、現在の位置指令と前回のサンプリ
ング時の位置とを比較して、加速度、定速、減速度の３
領域のいずれかを選択して最適の加速度、減速度を決め
てアクチュエータを自動的に制御できる。

【００１７】請求項４記載の発明の場合、オペレータの
操作によって長尺アーム先端に装着した吊り荷を所定の
位置に移動することを目的とした荷役機械におけるオペ
レータの操作によって生ずる指令値と該荷役機械の現在
値との差を求め、この差の時間に関する正の二次微分値
と、負の二次微分値と、零の二次微分値及び又は零の一
次微分値との３つのうちの値のうちの１つを選択してア
クチュエータの制御方法を切換えることにより、オペレ
ータが自由な意思でアクチュエータに速度指令又は位置
指令を与える場合に、現在の速度指令又は位置指令と前
回のサンプリング時のアクチュエータの速度又は位置と
を比較して、加加速度又は加速度、定加速又は定速、減
加速度又は減速度の３領域のいずれかを選択して最適の
加加速度又は加速度、減加速度又は減速度を決めてアク
チュエータを自動的に制御できる。

【００１８】請求項５記載の発明の場合、正の二次微分
値及び負の二次微分値を、それぞれ誤差の関数としてい
るため、アームの速度が速度図の指令値に正確に追従で
きる。

【００１９】

【実施例】以下、本発明に係る荷役機械の実施例につい
て説明する。図１には、本発明に係る荷役機械の制御方
法の実施例を示す制御ブロック図が示されている。

【００２０】図において、１は荷役機械を操作するオペ
レータの腕、２は荷役機械の荷役アームである。また、
３は操作レバーで荷役アーム２に取り付けられている。
４は力センサで操作レバー３に取り付けられており、オ
ペレータが操作レバー３を操作したときの動作速度、動
作力を検出するためのものである。したがって、オペレ
ータはオペレータの腕１によって、荷役機械の荷役アー
ム２に取り付けられた操作レバー３を用いて操作したい
速度に合った力を力センサ４に加える。

【００２１】５は指令変換器で、オペレータの腕１によ
って力センサ４に加えられたオペレータの指令力を指令
信号に変換し、荷役機械に衝撃を与えないように時々刻
々の加減速制御指令状態に合わせて加減速制御を行って
速度指令を出力するものである。６は電力増幅器、７は
サーボモータである。この電力増幅器６は、指令変換器
５からの速度指令信号に基づいて、サーボモータ７を駆
動する電力を供給するものである。また、サーボモータ
７は、電力増幅器６からの電力供給を受けて荷役機械の
荷役アーム２を動作させる。８は速度センサで、サーボ
モータ７の回転速度を検出し指令変換器５にフィードバ
ックするものである。

【００２２】次に指令変換器５の制御内容について説明
する。５１は速度比較器で、オペレータの操作によって
操作レバー３に加えた力による力センサ４からの指令値
と速度センサ８によって検出されるサーボモータ７の回
転速度に基づく現在の動作速度（フィードバック値）と
の差ΔＶを出力するものである。５２は第１判別器で、
速度比較器５１からの出力値ΔＶの値により以後の処理
方法を選択するための信号判別を行うもので、速度比較
器５１からの出力値ΔＶと速度偏差α（設定値で、最大
速度の１／30以下の値）の絶対値（｜α｜）とを比較
し、加加速（又は減加速）にすべきか否かを判定する。
すなわち、速度比較器５１からの出力値ΔＶが速度偏差
αの絶対値（｜α｜）に等しいか小さいときは、加加速
（又は減加速）にすべきでないと判定する。

【００２３】５３は第２判別器で、速度比較器５１から
出力されるΔＶの値と現在の加速度ａｉとの関係から加
加速あるいは減加速の判別を行うもので、予め定めたジ
ャーク値Ｊと速度比較器５１とから出力された２ＪΔＶ
と現在の加速度ａｉとを比較し、 ２ＪΔＶ≧ａｉ のときは、加加速（正のジャーク）となり、 ２ＪΔＶ＜ａｉ のときは、減加速（負のジャーク）となる。５４は加速
度計算器で、速度センサ８によって検出されたサーボモ
ータ７の回転速度データを記録して加速度ａｉを計算す
るものである。

【００２４】５５は速度偏差計算器で、第２判別器５３
の判別結果（正のジャークか、負のジャークか）と加速
度計算器５４からの出力値ａｉとから電力増幅器６に出
力する速度指令の基礎になる速度偏差ΔＶｉを求める。
５６は速度発生器で、第２判別器５３によって選択され
た加加速又は減加速のいずれかに基づいて速度偏差計算
器５５で計算された速度偏差値ΔＶｉ、又は第１判別器
５２の判別結果、加加速（又は減加速）すべきでないと
判定された場合の速度偏差値ΔＶｉ（＝０）のいずれか
一方を取り込んで、現在の速度（Ｖｉ_-1）に加算した制
御速度Ｖｉを算出し、電力増幅器６に出力するものであ
る。この速度偏差値ΔＶｉとジャークＪとの関係は、図
３に示す如き関係を有している。すなわち、速度偏差値
ΔＶｉの変化が所定値（０．６Ｖ）を超えると最大ジャ
ークを示すようになっている。

【００２５】次に本実施例の動作について説明する。図
１に図示の指令変換器５は、１サンプリング周期毎に動
作するように構成されている。この指令変換器５の動作
状況を、図２を用いて説明する。

【００２６】まず、オペレータが腕１によって荷役機械
の荷役アーム２に取り付けた操作レバーヘ３を操作して
力センサ４に力を加えて速度指令を与える。このオペレ
ータが力センサ４に図２に図示の速度図の速度指令値８
Ｖに示す速度指令を与えると、サンプリング時間１Δｔ
において、速度比較器５１は、力センサ４の出力速度指
令値８Ｖと速度センサ８からの出力値０とから偏差８Ｖ
を出力する。この速度比較器５１から出力される偏差８
Ｖは、第１判別器５２において速度偏差｜α｜と比較さ
れる。このαの値は、通常速度指令値Ｖの１／３以下に
設定される。したがって、第１判別器５２における比較
結果は、 ８Ｖ＞｜α｜ となり、第２判別器５３に入力される。第２判別器５３
においては、サンプリング時間１Δｔにおける速度Ｖが
０で、加速度ａｉ＝０であることから、（＋）Ｊのジャ
ーク（加加速）を選択する。

【００２７】この第２判別器５３における（＋）ジャー
クの選択により、速度偏差計算器５５には、図２のジャ
ーク図に示す如く、サンプリング時間１Δｔから次のサ
ンプリング時間２Δｔまでの間（＋）Ｊのジャークが出
力される。そして、この速度偏差計算器５５に入力され
る（＋）Ｊのジャークは、加速度計算器５４において積
分され、時々刻々の加速度が図２の加速度図に示すよう
に求まる。さらに、速度偏差計算器５５において図２に
図示の速度図に示す如き二次曲線を求め、この時々刻々
変化する加速偏差値に、速度発生器５６においてサンプ
リング時間１Δｔのときの速度Ｖ＝０（加加速状態であ
るため）を加えて電力増幅器６に出力される。図２に図
示のジャークＪは、荷役アーム２が振動しないでスムー
ズに追従していくよう決められているので、荷役アーム
２の速度は、図２に図示の速度図の指令値に追従して応
答していくようになっている。

【００２８】次に、サンプリング時間２Δｔに達する
と、図２に図示の速度図に示すように速度Ｖは、 となり、このときの加速度Ａは、図２に図示の加速度図
に示すように、 Ａ＝ＪΔｔ で示される。サンプリング時間２Δｔにおいて、荷役ア
ーム２は、図２に図示の速度図の指令値に追従して動作
し、オペレータの腕１は、正確に制御される。そして、
サンプリング時間２Δｔに達しても速度指令値は、サン
プリング時間１Δｔのときと変化していない。

【００２９】このサンプリング時間２Δｔにおいて、速
度比較器５１は、力センサ４の出力速度指令値８Ｖと速
度センサ８からの出力値１Ｖとから偏差７Ｖを出力す
る。この速度比較器５１から出力される偏差７Ｖは、第
１判別器５２において速度偏差｜α｜と比較される。そ
して、第１判別器５２における比較結果は、 ７Ｖ＞｜α｜ となり、第２判別器５３での処理を選択する。

【００３０】第２判別器５３では、まず、２ＪΔＶｉを
計算する。このΔＶｉは、 ΔＶｉ＝７Ｖ で、Ｖは、 であるから、ΔＶｉは、 となり、７Ｊ² Δｔ² と加速度ＪΔｔの二乗とを比較す
ると、 ７Ｊ² Δｔ² ＞Ｊ² Δｔ² となり、（＋）Ｊのジャークを選択する。この第２判別
器５３における（＋）ジャークの選択により、速度偏差
計算器５５には、図２のジャーク図に示す如く、サンプ
リング時間２Δｔから次のサンプリング時間３Δｔまで
の間（＋）Ｊのジャークが出力される。そして、この速
度偏差計算器５５に入力される（＋）Ｊのジャークは、
加速度計算器５４において積分され、時々刻々の加速度
が図２の加速度図に示すように求まる。

【００３１】さらに、速度偏差計算器５５において図２
に図示の速度図に示す如き二次曲線を求め、この時々刻
々変化する加速偏差値に、速度発生器５６においてサン
プリング時間２Δｔのときの速度Ｖ を加えて電力増幅器６に出力される。この間、荷役アー
ム２の速度は、図２に図示の速度図の指令値に正確に追
従する。さらに、サンプリング時間３Δｔに達すると、
図２に図示の速度図に示すように、速度は、４Ｖで、 ４Ｖ＝２ＪΔｔ² となり、このときの加速度は、図２に図示の加速度図に
示す如く２Ａで、 ２Ａ＝２ＪΔｔ で示される。

【００３２】サンプリング時間３Δｔにおいて、荷役ア
ーム２は、図２に図示の速度図の指令値に追従して動作
し、オペレータの腕１は、正確に制御される。そして、
サンプリング時間３Δｔに達しても速度指令値は、サン
プリング時間２Δｔのときと変化していない。すなわ
ち、オペレータの腕１は、力センサ４に出力８Ｖを出し
続けるよう制御されている。このサンプリング時間３Δ
ｔにおいて、速度比較器５１は、力センサ４の出力速度
指令値８Ｖと速度センサ８からの出力値４Ｖとから偏差
４Ｖを出力する。この速度比較器５１から出力される偏
差４Ｖは、第１判別器５２において速度偏差｜α｜と比
較される。そして、第１判別器５２における比較結果
は、 ４Ｖ＞｜α｜ となり、第２判別器５３での処理を選択する。

【００３３】第２判別器５３では、まず、２ＪΔＶｉを
計算する。このΔＶｉは、 ΔＶｉ＝４Ｖ で、Ｖは、 であるから、ΔＶｉは、 ΔＶｉ＝２ＪΔｔ² となり、４Ｊ² Δｔ² と加速度２ＪΔｔの二乗（２ＪΔ
ｔ）² とを比較すると、 ４Ｊ² Δｔ² ＝（２ＪΔｔ）² と等しくなり、（−）Ｊのジャーク（減加速）を選択す
る。この第２判別器５３における（−）ジャークの選択
により、速度偏差計算器５５には、図２のジャーク図に
示す如く、サンプリング時間３Δｔから次のサンプリン
グ時間４Δｔまでの間、（−）Ｊのジャークが出力され
る。そして、この速度偏差計算器５５に入力される
（−）Ｊのジャークは、加速度計算器５４において積分
され、時々刻々の加速度が図２の加速度図に示すように
求まる。

【００３４】さらに、速度偏差計算器５５において図２
に図示の速度図に示す如き二次曲線を求め、この時々刻
々変化する加速偏差値に、速度発生器５６においてサン
プリング時間３Δｔのときの速度Ｖ＝２ＪΔｔ² を加え
て電力増幅器６に出力される。この間、荷役アーム２の
速度は、図２に図示の速度図の指令値に正確に追従す
る。そして、サンプリング時間４Δｔに達すると、図２
に図示の速度図に示すように、速度は、７Ｖで、 となり、このときの加速度は、図２に図示の加速度図に
示す如くＡで、 Ａ＝ＪΔｔ で示される。

【００３５】サンプリング時間４Δｔにおいて、荷役ア
ーム２は、図２に図示の速度図の指令値に追従して動作
し、オペレータの腕１は、正確に制御される。そして、
サンプリング時間４Δｔに達しても速度指令値は、サン
プリング時間３Δｔのときと変化していない。すなわ
ち、オペレータの腕１は、力センサ４に出力８Ｖを出し
続けるよう制御されている。このサンプリング時間４Δ
ｔにおいて、速度比較器５１は、力センサ４の出力速度
指令値８Ｖと速度センサ８からの出力値７Ｖとから偏差
１Ｖを出力する。この速度比較器５１から出力される偏
差１Ｖは、第１判別器５２において速度偏差｜α｜と比
較される。そして、第１判別器５２における比較結果
は、 １Ｖ＞｜α｜ となり、第２判別器５３での処理を選択する。

【００３６】第２判別器５３では、まず、２ＪΔＶｉを
計算する。このΔＶｉは、 ΔＶｉ＝１Ｖ で、Ｖは、 であるから、ΔＶｉは、 となり、Ｊ² Δｔ² と加速度ＪΔｔの二乗とを比較する
と、 Ｊ² Δｔ² ＝（ＪΔｔ）² と等しくなり、（−）Ｊのジャーク（減加速）を選択す
る。この第２判別器５３における（−）ジャークの選択
により、速度偏差計算器５５には、図２のジャーク図に
示す如く、サンプリング時間４Δｔから次のサンプリン
グ時間５Δｔまでの間、（−）Ｊのジャークが出力され
る。そして、この速度偏差計算器５５に入力される
（−）Ｊのジャークは、加速度計算器５４において積分
され、時々刻々の加速度が図２の加速度図に示すように
求まる。

【００３７】さらに、速度偏差計算器５５において図２
に図示の速度図に示す如き二次曲線を求め、この時々刻
々変化する加速偏差値に、速度発生器５６においてサン
プリング時間４Δｔのときの速度Ｖ を加えて電力増幅器６に出力される。この間、荷役アー
ム２の速度は、図２に図示の速度図の指令値に正確に追
従する。また、サンプリング時間５Δｔに達すると、荷
役アーム２は速度指令値８Ｖと一致し、速度センサ８に
よる検出速度も図２に図示の速度図に示すように８Ｖに
なる。したがって、加速度は０となる。このとき速度比
較器５１からは、速度偏差０を出力する。第１判別器５
２は偏差０を速度偏差｜α｜と比較して、ΔＶｉ＝０を
選択し、その結果、速度発生器５６はサンプリング時間
５Δｔにおける速度８Ｖを電力増幅器６に出力する。そ
の結果サーボモータ７は速度８Ｖの一定速度で動作し続
ける。

【００３８】図４は、本発明に係る荷役機械の制御方法
の他の実施例を示す制御ブロック図で、図１に図示の実
施例と異なる点は、指令変換器５の制御方法の点であ
る。図４において、指令変換器５は以下のように構成さ
れる。

【００３９】１５１は速度比較器で、オペレータの操作
によって操作レバー３に加えた力による力センサ４から
の指令値と速度センサ８によって検出されるサーボモー
タ７の回転速度に基づく現在の動作速度（フィードバッ
ク値）との差ΔＶを出力するものである。１５２は第１
判別器で、速度比較器１５１からの出力値ΔＶの値と現
在の加速度ａｉの値により以後の処理方法を選択するた
めの信号判別を行うもので、速度比較器１５１からの出
力値ΔＶと速度偏差α（設定値で、最大速度の１／30以
下の値）の絶対値（｜α｜）とを比較すると共に、加速
度ａｉと加速度偏差βの絶対値（｜β｜）とを比較し、
加加速（又は減加速）か否かを判定する。すなわち、速
度比較器１５１からの出力値ΔＶが速度偏差αの絶対値
（｜α｜）に等しいか小さいときで、かつ加速度ａｉが
加速度偏差βの絶対値（｜β｜）に等しいか小さいとき
は、加加速（又は減加速）すべきでないと判定する。

【００４０】１５３は第２判別器で、速度比較器１５１
から出力されるΔＶの値と現在の加速度ａｉとの関係か
ら加加速あるいは減加速の判別を行うもので、速度比較
器１５１から出力された２ＪΔＶと現在の加速度ａｉ²
とを比較し、 ２ＪΔＶ≧ａｉ² のときは、加加速（正のジャーク）となり、 ２ＪΔＶ＜ａｉ のときは、減加速（負のジャーク）となる。１５４は加
速度計算器で、加速センサ８によって検出されたサーボ
モータ７の回転速度データを記録して加速度ａｉを計算
するものである。

【００４１】１５５は速度偏差計算器で、第２判別器１
５３の判別結果（正のジャークか、負のジャークか）と
加速度計算器１５４からの出力値ａｉとから電力増幅器
６に出力する速度指令の基礎になるΔＶｉを求める。１
５６は速度発生器で、第２判別器１５３によって選択さ
れた加加速又は減加速のいずれかに基づいて速度偏差計
算器１５５で計算された速度偏差値ΔＶｉ、又は第１判
別器１５２の判別結果、加加速（又は減加速）でないと
判定された場合の速度偏差値ΔＶｉ（＝０）のいずれか
一方を取り込んで、現在の速度（Ｖｉ_-1）に加算した制
御速度Ｖｉを算出し、電力増幅器６に出力するものであ
る。

【００４２】次に本実施例の動作について説明する。図
４の指令変換器５も１サンプリング周期毎に動作するが
その状況は図２のような速度指令値をとるときには図１
の指令変換器５と全く同じ動作を行う。速度指令値が図
５の場合には、サンプリング時間２Δｔまでは、図５は
図２と同じ速度指令値であるから指令変換器５の動作は
図２と全く変わらない。さらに、サンプリング時間３Δ
ｔに達すると、図５に図示の速度図に示すように、速度
は、４Ｖで、 ４Ｖ＝２ＪΔｔ² となり、このときの加速度は、図５に図示の加速度図に
示す如く２Ａで、 ２Ａ＝２ＪΔｔ で示される。

【００４３】サンプリング時間３Δｔに達するまでは、
荷役アーム２は、図５に図示の速度図の指令値に追従し
て動作し、オペレータの腕１は、正確に制御される。そ
して、サンプリング時間３Δｔに達したとき、速度指令
値は、サンプリング時間２Δｔの８Ｖに比較して下げて
７．５Ｖに変化させている。そこで、速度比較器１５１
は、このサンプリング時間３Δｔにおいて、力センサ４
の出力速度指令値７．５Ｖと速度センサ８からの出力値
４Ｖとから偏差３．５Ｖを出力する。この速度比較器１
５１から出力される偏差３．５Ｖは、第１判別器１５２
において速度偏差｜α｜と比較される。そして、第１判
別器１５２における比較結果は、 ３．５Ｖ＞｜α｜ となり、第２判別器１５３での処理を選択する。

【００４４】第２判別器１５３では、まず、２ＪΔＶｉ
を計算する。このΔＶｉは、 ΔＶｉ＝３．５Ｖ で、Ｖは、 であるから、ΔＶｉは、 ΔＶｉ＝１．７５ＪΔｔ² となり、３．５Ｊ² Δｔ² と加速度２ＪΔｔの二乗（２
ＪΔｔ）² とを比較すると、 ３．５Ｊ² Δｔ² ＜（２ＪΔｔ）² と小さくなり、（−）Ｊのジャーク（減加速）を選択す
る。

【００４５】この第２判別器１５３における（−）ジャ
ークの選択により、速度偏差計算器１５５には、図５の
ジャーク図に示す如く、サンプリング時間３Δｔから次
のサンプリング時間４Δｔまでの間、（−）Ｊのジャー
クが出力される。そして、この速度偏差計算器１５５に
入力される（−）Ｊのジャークは、加速度計算器１５４
において積分され、時々刻々の加速度が図５の加速度図
に示すように求まる。さらに、速度偏差計算器１５５に
おいて図５に図示の速度図に示す如き二次曲線を求め、
この時々刻々変化する加速偏差値に、速度発生器１５６
においてサンプリング時間３Δｔのときの速度Ｖ＝２Ｊ
Δｔ² を加えて電力増幅器６に出力される。この間、荷
役アーム２の速度は、図５に図示の速度図の指令値に正
確に追従する。

【００４６】そして、サンプリング時間４Δｔに達する
と、図５に図示の速度図に示すように、速度は、７Ｖ
で、 となり、このときの加速度は、図２に図示の加速度図に
示す如くＡで、 Ａ＝ＪΔｔ で示される。サンプリング時間４Δｔにおいて、荷役ア
ーム２は、図５に図示の速度図の指令値に追従して動作
し、オペレータの腕１は、正確に制御される。そして、
サンプリング時間４Δｔに達すると速度指令値は、サン
プリング時間３Δｔのときの７．５Ｖと比較して下げた
７Ｖに変化している。

【００４７】このサンプリング時間４Δｔにおいて、速
度比較器１５１は、力センサ４の出力速度指令値７Ｖと
速度センサ８からの出力値７Ｖとから偏差０を出力す
る。この速度比較器１５１から出力される偏差０は、第
１判別器１５２において速度偏差｜α｜と比較される。
そして、第１判別器１５２における比較結果は、 ０Ｖ＜｜α｜ となる。一方で加速度ａｉ＝Ａであるから、 Ａ＞｜β｜ とな、結局、第２判別器１５３での処理を選択する。第
２判別器１５３では、まず、２ＪΔＶｉを計算する。こ
のΔＶｉは、 ΔＶｉ＝０ となり、このΔＶｉと加速度ＪΔｔの二乗とを比較する
と、 ０＜（ＪΔｔ）² となる。そこで第２判別器１５３においては、（−）Ｊ
のジャーク（減加速）を選択する。

【００４８】この第２判別器１５３における（−）ジャ
ークの選択により、速度偏差計算器１５５には、図５の
ジャーク図の破線に示す如く、サンプリング時間４Δｔ
から次のサンプリング時間５Δｔまでの間、（−）Ｊの
ジャークが出力される。そして、この速度偏差計算器１
５５に入力される（−）Ｊのジャークは、加速度計算器
１５４において積分され、時々刻々の加速度が図５の加
速度図の破線に示すように求まる。さらに、速度偏差計
算器１５５において図５に破線で図示の速度図に示す如
き二次曲線を求め、この時々刻々変化する加速偏差値
に、速度発生器１５６においてサンプリング時間４Δｔ
のときの速度 を加えて電力増幅器６に出力される。この間、荷役アー
ム２の速度は、図５に破線で図示の速度図の指令値に正
確に追従する。また、サンプリング時間５Δｔに達する
と、図５に破線で図示の速度図に示すように荷役アーム
２は、速度指令値をサンプリング時間４Δｔの７Ｖに比
較して下げて６．５Ｖに変化させている。速度センサ８
による検出速度は、図５に破線で図示の速度図に示すよ
うに８Ｖになる。加速度は、図５の加速図に破線で示す
ように０となる。このとき速度比較器１５１からは、速
度偏差ΔＶ＝１．５Ｖを出力する。この速度比較器１５
１から出力される偏差１．５Ｖは、第１判別器１５２に
おいて速度偏差｜α｜と比較される。そして、第１判別
器１５２における比較結果は、 １．５Ｖ＞｜α｜ となり、第２判別器１５３での処理を選択する。第２判
別器１５３では、まず、２ＪΔＶｉを計算する。このΔ
Ｖｉは、 ΔＶｉ＝−１．５Ｖ で、Ｖは、 であるから、ΔＶｉは、 ΔＶｉ＝−０．７５ＪΔｔ² となり、−１．５Ｊ² Δｔ² と図５に破線で図示の加速
度０の二乗（０）² とを比較すると、 −１．５Ｊ² Δｔ² ＜０ と小さくなり、（−）Ｊのジャーク（減加速）を選択す
る。

【００４９】この第２判別器１５３における（−）ジャ
ークの選択により、速度偏差計算器１５５には、図５の
ジャーク図の破線に示す如く、サンプリング時間５Δｔ
から次のサンプリング時間６Δｔまでの間、（−）Ｊの
ジャークが出力される。そして、この速度偏差計算器１
５５に入力される（−）Ｊのジャークは、加速度計算器
１５４において積分され、時々刻々の加速度が図５の加
速度図の破線に示すように求まる。さらに、速度偏差計
算器１５５において図５に図示の速度図の破線に示す如
き二次曲線を求め、この時々刻々変化する加速偏差値
に、速度発生器１５６においてサンプリング時間５Δｔ
のときの速度Ｖ＝８Ｖを加えて電力増幅器６に出力され
る。サンプリング時間２Δｔから下げ出した力センサ４
より出力される速度指令値は、サンプリング時間５Δｔ
からさらに下げサンプリング時間６Δｔにおいては、サ
ンプリング時間５Δにおける速度指令値６．５Ｖよりさ
らに下げて６Ｖにしている。したがって、サンプリング
時間６Δｔに達すると、図５に図示の速度図の破線に示
すように、速度は、７．２Ｖで、 となり、このときの加速度は、図５に図示の加速度図に
示す如く（−）Ａで、 −Ａ＝−ＪΔｔ で示される。

【００５０】サンプリング時間６Δｔにおいて、荷役ア
ーム２は、図５に図示の速度図の指令値に追従して動作
し、オペレータの腕１は、正確に制御される。そして、
サンプリング時間６Δｔに達しても速度指令値は、サン
プリング時間５Δｔのときよりも低い値に変化してい
る。すなわち、オペレータの腕１は、力センサ４に出力
６Ｖにまで下げた値を出力するよう制御している。この
サンプリング時間６Δｔにおいて、速度比較器１５１
は、力センサ４の出力速度指令値６Ｖと、速度センサ８
からの出力値７．２Ｖとから偏差１．２Ｖを出力する。
この速度比較器１５１から出力される偏差１．２Ｖは、
第１判別器１５２において速度偏差｜α｜と比較され
る。そして、第１判別器１５２における比較結果は、 １．２Ｖ＞｜α｜ となり、第２判別器１５３での処理が選択される。第２
判別器１５３では、まず、２ＪΔＶｉを計算する。この
ΔＶｉは、 ΔＶｉ＝−１．２Ｖ で、Ｖは、 で、Ｊが（−）であるから、２ＪΔＶｉは、 ２ＪΔＶｉ＝１．２Ｊ² Δｔ² となる。このときの加速度ａｉは、図５に図示の加速度
図に示す如く−Ａで、 ａｉ＝−Ａ＝−ＪΔｔ となる。

【００５１】そこで、１．２Ｊ² Δｔ² と加速度（−）
ＪΔｔの二乗（−ＪΔｔ）² とを比較すると、 １．２Ｊ² Δｔ² ＞Ｊ² Δｔ² と大きくなり、（＋）Ｊのジャーク（加加速）を選択す
る。

【００５２】この第２判別器１５３における（＋）ジャ
ークの選択により、速度偏差計算器１５５には、図５の
ジャーク図の破線に示す如く、サンプリング時間６Δｔ
から次のサンプリング時間７Δｔまでの間、（＋）Ｊの
ジャークが出力される。そして、この速度偏差計算器１
５５に入力される（＋）Ｊのジャークは、加速度計算器
１５４において積分され、時々刻々の加速度が図５の加
速度図の破線に示すように求まる。さらに、速度偏差計
算器１５５において図５に図示の速度図の破線に示す如
き二次曲線を求め、この時々刻々変化する加速偏差値
に、速度発生器１５６においてサンプリング時間６Δｔ
のときの速度Ｖ＝７．２Ｖを加えて電力増幅器６に出力
される。サンプリング時間２Δｔから下げ出した力セン
サ４より出力される速度指令値は、サンプリング時間６
Δｔからさらに下げサンプリング時間７Δｔにおいて
は、サンプリング時間５Δにおける速度指令値６Ｖより
さらに下げて５．５Ｖにしている。このため、オペレー
タの腕１は、力センサ４に出力５．５Ｖにまで下げた値
を出力するよう制御している。このサンプリング時間７
Δｔにおいて、速度比較器１５１は、力センサ４の出力
速度指令値５．５Ｖと速度センサ８からの出力値５．９
Ｖとから偏差０．４Ｖを出力する。この速度比較器１５
１から出力される偏差０．４Ｖは、第１判別器１５２に
おいて速度偏差｜α｜と比較される。そして、第１判別
器１５２における比較結果は、 ０．４Ｖ＞｜α｜ となり、第２判別器１５３での処理が選択される。

【００５３】第２判別器１５３では、まず、２ＪΔＶｉ
を計算する。このΔＶｉは、 ΔＶｉ＝０．４Ｖ で、Ｖは、 であるから、ΔＶｉは、 ΔＶｉ＝０．２ＪΔｔ² となる。そして、この２ＪΔＶｉ（＝０．４Ｊ² Δ
ｔ² ）と、加速度（−）ＪΔｔの二乗（ＪΔｔ）² とを
比較すると、 ０．４Ｊ² Δｔ² ＜Ｊ² Δｔ² となり、小さくなり、（−）Ｊのジャーク（減加速）を
選択する。

【００５４】この第２判別器１５３における（−）ジャ
ークの選択により、速度偏差計算器１５５には、図５の
ジャーク図の破線に示す如く、サンプリング時間７Δｔ
から次のサンプリング時間８Δｔまでの間、（−）Ｊの
ジャークが出力される。そして、この速度偏差計算器１
５５に入力される（−）Ｊのジャークは、加速度計算器
１５４において積分され、時々刻々の加速度が図５の加
速度図の破線に示すように求まる。さらに、速度偏差計
算器１５５において図５に図示の速度図の破線に示す如
き二次曲線を求め、この時々刻々変化する加速偏差値
に、速度発生器１５６においてサンプリング時間７Δｔ
のときの速度Ｖ＝５．９Ｖを加えて電力増幅器６に出力
される。サンプリング時間２Δｔから下げ出した力セン
サ４より出力される速度指令値は、サンプリング時間７
Δｔからさらに下げサンプリング時間８Δｔにおいて
は、サンプリング時間７Δにおける速度指令値５．５Ｖ
よりさらに下げて５．３Ｖにしている。サンプリング時
間７Δから次のサンプリング時間８Δｔまでの間の
（−）Ｊジャークは、速度偏差ΔＶ＝０．４Ｖであった
ために図に示す如く、Ｊの値がΔＶの関数関係から小さ
くなっているため、減速が緩やかであり、サンプリング
時間１０Δｔにおいては、速度指令値５．３Ｖに近くな
り、定速動作となる状況にある。

【００５５】図１の実施例で、速度指令値が図５の場合
は、図１と図４の動作は、サンプリング時間４Δｔまで
は、全く変わらない。その理由は、速度指令値と速度セ
ンサ８からの出力値との偏差が大きく図１と図４の共通
部分の制御フローを通っていたからである。しかし、サ
ンプリング時間４Δｔにおいては、速度比較器５１は力
センサ４の出力速度指令値７Ｖと速度センサ８からの出
力７Ｖとが一致して偏差０Ｖとなることから図１と図４
の動作の間に差を生ずる。

【００５６】図１では、速度比較器５１から出力される
偏差０Ｖは、第１判別器５２において速度偏差｜α｜と
比較される。そして第１判別器５２における比較結果
は、 ０Ｖ＜｜α｜ となり、第２判別器５３での処理は選択されない。すな
わち、加速度ａｉが ａｉ＝０ で、ジャークＪ＝０が選択される。しかし、サンプリン
グ時間４Δｔにおいては、加速度がＡ（＝ＪΔｔ）であ
るにも拘らず、力センサ４の出力速度指令値と速度セン
サ８からの出力値との偏差を示すＶｉが、Ｖｉ＝０であ
ので速度偏差計算器５５においては、速度を０に収束さ
せるために、速度偏差ΔＶｉにΔＶｉ＝０を選択するこ
とになる。速度偏差ΔＶｉ＝０を選択すると、早期に速
度を０にしようと急激な加速度の減少を起こし加速度が
０になるまでの間、図５に実線で図示のジャーク図に示
すように（−）側の最大のジャークを発生させる。加速
度が０になった後は、図５に実線で図示のジャーク図に
示すようにジャークＪは０になる。この結果、荷役アー
ム２の速度は、実線に示すように７．１Ｖで定速動作す
るように変化する。また、サンプリング時間５Δｔに達
すると、図５に図示の速度図に示すように、速度は、
７．１Ｖで、 ７．１Ｖ＝３．５５ＪΔｔ² となり、このときの加速度は、図５に実線で図示の加速
度図に示す如く０である。サンプリング時間５Δｔにお
いて、荷役アーム２は、図５に図示の速度図の指令値に
追従して動作し、オペレータの腕１は、正確に制御され
る。そして、サンプリング時間５Δｔに達したとき、速
度指令値は、サンプリング時間４Δｔの７Ｖに比較して
下げて６．５Ｖに変化させている。

【００５７】そこで、速度比較器５１は、このサンプリ
ング時間５Δｔにおいて、力センサ４の出力速度指令値
６．５Ｖと速度センサ８からの出力値７．１Ｖとから偏
差０．６Ｖを出力する。この速度比較器５１から出力さ
れる偏差０．６Ｖは、第１判別器５２において速度偏差
｜α｜と比較される。そして、第１判別器５２における
比較結果は、 ０．６Ｖ＞｜α｜ となり、第２判別器５３での処理を選択する。第２判別
器５３では、まず、２ＪΔＶｉを計算する。このΔＶｉ
は、 ΔＶｉ＝−０．６Ｖ で、Ｖは、 であるから、ΔＶｉは、 ΔＶｉ＝−０．３ＪΔｔ² となり、０．３Ｊ² Δｔ² と図５に実線で図示の加速度
０（零）の二乗（０）²とを比較すると、 −０．６Ｊ² Δｔ² ＜０ と小さくなり、（−）Ｊのジャーク（減加速）を選択す
る。

【００５８】この第２判別器５３における（−）ジャー
クの選択により、速度偏差計算器５５には、図５のジャ
ーク図の実線に示す如く、サンプリング時間５Δｔから
次のサンプリング時間６Δｔまでの間、（−）Ｊのジャ
ークが出力される。そして、この速度偏差計算器５５に
入力される（−）Ｊのジャークは、加速度計算器５４に
おいて積分され、時々刻々の加速度が図５の加速度図の
実線に示すように求まる。さらに、速度偏差計算器５５
において図５に図示の速度図の実線に示す如き二次曲線
を求め、この時々刻々変化する加速偏差値に、速度発生
器５６においてサンプリング時間５Δｔのときの速度Ｖ
＝７．１Ｖを加えて電力増幅器６に出力される。サンプ
リング時間２Δｔから下げ出した力センサ４より出力さ
れる速度指令値は、サンプリング時間５Δｔからさらに
下げサンプリング時間６Δｔにおいては、サンプリング
時間５Δにおける速度指令値６．５Ｖよりさらに下げて
６Ｖにしている。したがって、サンプリング時間６Δｔ
に達すると、図５に図示の速度図に示すように、速度
は、６．２Ｖで、 となり、このときの加速度は、図５に図示の加速度図に
示す如く（−）Ａで、 −Ａ＝−ＪΔｔ で示される。

【００５９】サンプリング時間６Δｔにおいて、荷役ア
ーム２は、図５に図示の速度図の指令値に追従して動作
し、オペレータの腕１は、正確に制御される。そして、
サンプリング時間６Δｔに達しても速度指令値は、サン
プリング時間５Δｔのときよりも低い値に変化してい
る。すなわち、オペレータの腕１は、力センサ４に出力
６Ｖにまで下げた値を出力するよう制御している。この
サンプリング時間６Δｔにおいて、速度比較器５１は、
力センサ４の出力速度指令値６Ｖと速度センサ８からの
出力値６．２Ｖとから偏差０．２Ｖを出力する。この速
度比較器５１から出力される偏差０．２Ｖは、第１判別
器５２において速度偏差｜α｜と比較される。そして、
第１判別器５２における比較結果は、α＝１／３Ｖとし
ているので、 ０．２Ｖ＜｜α｜ となり、第２判別器１５３での処理は選択されない。す
なわち、 ΔＶｉ＝０ が選択される。これにより速度発生器５６は電力増幅器
６に速度６．２Ｖを出力する。

【００６０】しかし、サンプリング時間６Δｔにおいて
は、加速度が（−）Ａ（＝−ＪΔｔ）であるにも拘ら
ず、力センサ４の出力速度指令値と速度センサ８からの
出力値との偏差を示すΔＶｉが、ΔＶｉ＝０であるの
で、早期に加速度を０（零）にしようと（−）Ａから急
激な加速度の増加を起こし加速度が０（零）になるまで
の間、図５に図示のジャーク図の実線に示すように
（＋）側の最大のジャークを発生させる。加速度が０
（零）になった後は、図５に図示のジャーク図の実線に
示すようにジャークＪは０（零）になる。この結果、荷
役アーム２の速度は、実線に示すように５．８Ｖで定速
動作するように変化する。そして、サンプリング時間７
Δｔに達すると、図５に図示の速度図に示すように速度
は、５．７Ｖで、 となり、このときの加速度は、図５に図示の加速度図に
示す如く０で示される。サンプリング時間７Δｔにおい
て、荷役アーム２は、図５に図示の速度図の指令値に追
従して動作し、オペレータの腕１は、正確に制御され
る。そして、サンプリング時間７Δｔに達しても速度指
令値は、サンプリング時間６Δｔのときよりも低い値
５．５Ｖに変化している。このため、オペレータの腕１
は、力センサ４に出力５．５Ｖにまで下げた値を出力す
るよう制御している。

【００６１】このサンプリング時間７Δｔにおいて、速
度比較器５１は、力センサ４の出力速度指令値５．５Ｖ
と速度センサ８からの出力値５．７Ｖとから偏差０．２
Ｖを出力する。この速度比較器５１から出力される偏差
０．２Ｖは、第１判別器１５２において速度偏差｜α｜
と比較される。そして、第１判別器１５２における比較
結果は、 ０．２Ｖ＜｜α｜ となり、第２判別器５３での処理は選択されない。すな
わち、速度ΔＶｉが ΔＶｉ＝０ が選択される。このときの加速度が０であるから速度発
生器５６は電力増幅器６に速度５．７Ｖを出力する。そ
して、荷役アーム２は一定速度５．６Ｖで動作し続け
る。なお、図５のサンプリング時間８Δｔにおいて速度
指令値が５．３Ｖでアーケム２の速度が５．６Ｖである
から、その偏差−０．３Ｖは、図１の実施例では誤差と
して生じたままとなる。

【００６２】図６〜図７には、本発明に係る制御方法の
適用されるマニピュレータの概略構成及び制御ブロック
図が示されている。図において、１１はマスターアー
ム、１２はスレーブアームで、このスレーブアーム１２
は、マスターアーム１１とは異なる形状に構成されてい
る。１３はマニピュレータコントローラで、マスターア
ーム１１の動きによってスレーブアーム１２を動作させ
るための座標変換を行う。これらマスターアーム１１及
びスレーブアーム１２は、実際のものは６自由度を持つ
が説明を簡単にするため３自由度としてある。１４、１
５、１６、はマスターアーム１１の自由度に取り付けら
れた位置検出センサで、１７はマスターアーム１１の先
端に取り付けられたグリップである。オペレータはグリ
ップ１７を握ってマスターアーム１１を動かすと、マス
ターアーム１１の各可動部に取り付けられた位置検出セ
ンサ１４、１５、１６が動き位置検出センサ１４、１
５、１６の移動方向と移動量がマニピュレータコントロ
ーラ１３に送られる。

【００６３】１８は操作信号処理部であり、位置検出セ
ンサ１４、１５、１６の出力信号の故障検出、位相弁別
を行った後、出力パルスをカウントする。１９は経路演
算部で、マスターアーム１１とスレーブアーム１２の幾
何学的形状を基にそれぞれの先端が相似の動作になるよ
う演算する。これによりマスターアーム１１とスレーブ
アーム１２の構造が異なっていてもグリップ１７とスレ
ーブアーム１２の先端の動作は幾何学的に相似になるよ
うスレーブアーム１２の各自由度のアクチュエータに信
号を送ることができる。

【００６４】２０、２１、２２はスレーブアーム１２の
３自由度のアクチュエータである。それぞれのアクチュ
エータ２０、２１、２２には、それぞれ位置検出センサ
２３、２４、２５が取り付けられており、スレーブアー
ム１２の位置が検出できるようになっている。２６はサ
ーボ制御部で経路演算部１９からの制御信号を増幅して
アクチュエータ２０、２１、２２に送ると共に位置検出
センサ２３、２４、２５からのフィードバック信号を受
けて経路演算部１９の制御信号と比較して更に偏差を出
力する。２７は画像入力装置でありスレーブアーム１２
の先端近くに設置される。２８は画像処理装置であり、
２９はグラフィックディスプレーで画像処理装置２８の
信号を表示するオペレータはマスターアーム１１を操作
するときグラフィックディスプレー２９を見て行いマス
ターアーム１１の動作を意識する必要はない。

【００６５】次に本実施例の動作について説明する。マ
スターアーム１１の自由度に取り付けられた位置検出セ
ンサ１４、１５、１６で検出した移動方向と移動量は、
マニピュレータコントローラ１３の操作信号処理部１８
に入力される。この操作信号処理部１８は以下のように
構成される。ここで位置検出は、Ｘ軸方向、Ｙ軸方向、
Ｚ軸方向の三次元の位置を検出するものであるが、説明
の都合上Ｘ軸の制御だけを取り上げている。また、本実
施例では３自由度を例に上げているため、位置検出セン
サも３個用いており、それらのそれぞれについて位置検
出を行って位置制御を行うのであるが、ここでは説明の
都合上１個の位置検出センサについてだけを取り上げて
いる。

【００６６】８１は位置比較器で、オペレータの操作に
よって移動したマスターアーム１１の位置を検出する位
置検出センサ１４による位置検出値と、マスターアーム
１１の動作によって移動したスレーブアーム１２の位置
を検出する位置検出センサ２０による位置検出値とを比
較して、その差ΔＸを出力するものである。

【００６７】８２は第１判別器で、位置比較器８１から
の出力値ΔＸの値により以後の処理方法を選択するため
の信号判別を行うもので、位置比較器８１からの出力値
ΔＸと位置偏差α（設定値で、Ｘの１／３以下の値）の
絶対値（｜α｜）とを比較し、加速（又は減速）か否か
を判定する。すなわち、位置比較器８１からの出力値Δ
Ｘが速度偏差αの絶対値（｜α｜）より大きい場合に、
加速と判定し、位置比較器８１からの出力値ΔＸが速度
偏差αの絶対値（｜α｜）に等しいか小さい場合に減速
であると判定する。

【００６８】８３は第２判別器で、位置比較器８１から
出力されるΔＸの値と現在の速度Ｖｉとの関係から加速
度（Ａ）あるいは減速度（−Ａ）の判別を行うもので、
位置比較器８１から出力された２ＡΔＸと現在の速度Ｖ
ｉとを比較し、 ２ＡΔＸ＞Ｖｉ² のときは、加速度となり、 ２ＡΔＸ≦Ｖｉ² のときは、減速度となる。

【００６９】８４は速度計算器で、位置検出センサ２３
によって検出されたアクチュエータ２０の回転位置デー
タを記録して速度Ｖｉを計算するものである。

【００７０】８５は位置偏差計算器で、第２判別器８３
の判別結果（正の加速度か、負の加速度か）と速度計算
器８４からの出力値Ｖｉとから操作信号処理部１９に出
力する速度指令の基礎になる位置偏差ΔＸｉを求める。

【００７１】８６は位置発生器で、第２判別器８３によ
って選択された加速又は減速のいずれかに基づいて位置
偏差計算器８５で計算された位置偏差値ΔＸｉ、又は第
１判別器８２の判別結果、加速でないと判定された場合
の位置偏差値ΔＸｉ（＝０）のいずれか一方を取り込ん
で、現在の位置（Ｘｉ_-1）に加算した制御位置Ｘｉを算
出し、操作信号処理部１９に出力するものである。

【００７２】

【発明の効果】請求項１記載の発明によれば、オペレー
タの操作に基づいて吊り荷を所定の位置に移動すること
を目的とした荷役機械におけるオペレータの操作に基づ
く指令値と荷役機械の現在値との差を求め、この差の時
間に関する正の二次微分値と負の二次微分値と零の微分
値の３つのうち１つを選択してアクチュエータの制御方
法を切換えるようにしてあるため、オペレータが自由な
意思でアクチュエータに速度指令又は位置指令を与える
場合に、現在の速度と前回のサンプリング速度とを比較
して、加加速又は加速度、定加速又は定速、減加速又は
減速度の３領域のいずれかを選択して最適の加加速度又
は加速度、減加速度又は減速度を決めてアクチュエータ
を自動的に制御することができる。

【００７３】請求項２記載の発明によれば、オペレータ
の操作によって長尺アーム先端に装着した吊り荷を所定
の位置に移動することを目的とした荷役機械におけるオ
ペレータの操作によって生ずる長尺アームの速度指令値
と該荷役機械の長尺アームの現在の速度との差を求め、
この差の値に基づいて正のジャーク値と、負のジャーク
値と、零の加速度値及び又はジャーク値の３つの値のう
ちの１つを選択してアクチュエータの制御方法を切換え
るようになっているため、オペレータが自由な意思でア
クチュエータに速度指令を与える場合に、現在の速度指
令と前回のサンプリング時のアクチュエータの速度とを
比較して、加加速、定加速又は定速、減加速度の３領域
のいずれかを選択して最適の加加速度、減加速度を決め
てアクチュエータを自動的に制御することができる。

【００７４】請求項３記載の発明によれば、オペレータ
の操作によって長尺アーム先端に装着した吊り荷を所定
の位置に移動することを目的とした荷役機械におけるオ
ペレータの操作によって生ずる長尺アームの位置指令値
と該荷役機械の長尺アームの現在の位置との差を求め、
この差の値に基づいて正の加速度値と、負の加速度値
と、零の加速度値との３つの値のうちの１つを選択して
アクチュエータの制御方法を切換えるようになっている
ため、オペレータが自由な意思でアクチュエータに位置
指令を与える場合に、現在の位置指令と前回のサンプリ
ング時のアクチュエータの位置とを比較して、加速度、
定速、減速度の３領域のいずれかを選択して最適の加速
度、減速度を決めてアクチュエータを自動的に制御する
ことができる。

【００７５】請求項４記載の発明によれば、オペレータ
の操作によって長尺アーム先端に装着した吊り荷を所定
の位置に移動することを目的とした荷役機械におけるオ
ペレータの操作によって生ずる指令値と該荷役機械の現
在値との差を求め、この差の時間に関する正の二次微分
値と、負の二次微分値と、零の二次微分値及び又は零の
一次微分値との３つのうちの値のうちの１つを選択して
アクチュエータの制御方法を切換えるようになっている
ため、オペレータが自由な意思でアクチュエータに速度
指令又は位置指令を与える場合に、現在の指令値と前回
のサンプリング時のアクチュエータの位置又は速度とを
比較して、加加速又は加速度、定加速又は定速、減加速
又は減速度の３領域のいずれかを選択して最適の加加速
度又は加速度、減加速度又は減速度を決めてアクチュエ
ータを自動的に制御することができる。

【００７６】請求項５記載の発明によれば、正の二次微
分値及び負の二次微分値を、それぞれ誤差の関数として
いるため、アームの速度が速度図の指令値に正確に追従
することができる。

【図面の簡単な説明】

【図１】本発明に係る荷役機械の制御方法の実施例を示
す制御ブロック図である。

【図２】図１に図示の荷役機械の制御方法による荷役ア
ームの動作時のジャーク図、加速度図、速度図を示す図
である。

【図３】図１に図示の速度偏差値に対するジャーク値を
示す図である。

【図４】本発明に係る荷役機械の制御方法の他の実施例
を示す制御ブロック図である。

【図５】図４に図示の荷役機械の制御方法によるマニピ
ュレータの動作時のジャーク図、加速度図、速度図を示
す図である。

【図６】本発明に係る荷役機械の制御方法の適用される
マニピュレータのシステム構成図である。

【図７】本発明に係る荷役機械の制御方法の別な実施例
を示す制御ブロック図である。

【図８】従来のマニピュレータのシステム構成図であ
る。

【符号の説明】

１…………………………………………………オペレータ
の腕 ２…………………………………………………荷役アーム ３…………………………………………………操作レバー ４…………………………………………………力センサ ５…………………………………………………指令変換器 ５１………………………………………………速度比較器 ５２，１５２……………………………………第１判別器 ５３，１５３……………………………………第２判別器 ５４，１５４……………………………………加速度計算
器 ５５，１５５……………………………………速度偏差計
算器 ５６，１５６……………………………………速度発生器 ６…………………………………………………電力増幅器 ７…………………………………………………サーボモー
タ ８…………………………………………………速度センサ １５１……………………………………………速度比較器 １１………………………………………………マスターア
ーム １２………………………………………………スレーブア
ーム １３………………………………………………マニピュレ
ータコントローラ １４，１５，１６………………………………位置検出セ
ンサ １８………………………………………………操作信号処
理部 １９………………………………………………経路演算部 ２０，２１，２２………………………………アクチュエ
ータ ２３，２４，２５………………………………位置検出セ
ンサ

Claims (5)

【特許請求の範囲】
1. 【請求項１】 オペレータの操作に基づいて吊り荷を所
   定の位置に移動することを目的とした荷役機械におい
   て、上記オペレータの操作に基づく指令値と荷役機械の
   現在値との差を求め、該差の時間に関する正の二次微分
   値と負の二次微分値と零の微分値の３つのうち１つを選
   択してアクチュエータの制御方法を切換えることを特徴
   とする荷役機械の制御方法。
2. 【請求項２】 オペレータの操作によって長尺アーム先
   端に装着した吊り荷を所定の位置に移動することを目的
   とした荷役機械において、オペレータの操作によって生
   ずる長尺アームの速度指令値と該荷役機械の長尺アーム
   の現在の速度との差を求め、該差の値に基づいて正のジ
   ャーク値と、負のジャーク値と、零の加速度値及び又は
   ジャーク値の３つの値のうちの１つを選択してアクチュ
   エータの制御方法を切換えることを特徴とする荷役機械
   の制御方法。
3. 【請求項３】 オペレータの操作によって長尺アーム先
   端に装着した吊り荷を所定の位置に移動することを目的
   とした荷役機械において、オペレータの操作によって生
   ずる長尺アームの位置指令値と該荷役機械の長尺アーム
   の現在の位置との差を求め、該差の値に基づいて正の加
   速度値と、負の加速度値と、零の加速度値との３つの値
   のうちの１つを選択してアクチュエータの制御方法を切
   換えることを特徴とする荷役機械の制御方法。
4. 【請求項４】 オペレータの操作によって長尺アーム先
   端に装着した吊り荷を所定の位置に移動することを目的
   とした荷役機械において、オペレータの操作によって生
   ずる指令値と該荷役機械の現在値との差を求め、該差の
   時間に関する正の二次微分値と、負の二次微分値と、零
   の二次微分値及び又は零の一次微分値との３つのうちの
   値のうちの１つを選択してアクチュエータの制御方法を
   切換えることを特徴とする荷役機械の制御方法。
5. 【請求項５】 上記正の二次微分値及び負の二次微分値
   は、それぞれ誤差の関数であることを特徴とする請求項
   １又は４記載の荷役機械の制御方法。