(57)【特許請求の範囲】
【請求項１】
その移動経路が予測できないような移動物体に対して制御対象の作用端を接近させたりあるいは移動物体の軌跡に追従させるための、移動物体への接近及び／又は追従の制御に係るサーボ制御装置において、
移動物体への接近動作や追従動作に係る制御の開始指令を発する指令手段と、
移動物体の運動状態を検出する移動物体検出手段と、
指令手段からの上記開始指令を受けた場合に、移動物体検出手段からの移動物体の位置情報と、制御対象の作用端の位置情報とに基づいて、両者間の差をゼロにするために制御対象の作用端への制御指令値を計算する制御指令値計算手段と、
制御指令値計算手段からの制御指令値と、制御対象に係る加減速曲線において速度がピーク値に達するまでの時間（以下、「ピーク速度到達時間」という。）との関係を規定するとともに、制御指令値からピーク速度到達時間の値を決定するピーク速度到達時間値決定手段と、
上記ピーク速度到達時間をその基準値で割ることによって無次元量として定義される時間軸伸縮用パラメータ値を決定する時間軸伸縮用パラメータ値決定手段と、
制御指令値計算手段からの制御指令値に基づいて位置指令に係るランプ信号の折れ時間を求める際に、ランプ信号の傾斜つまり速度が制御対象に係る最大速度未満の場合にはランプ信号の折れ時間をピーク速度到達時間と同じ値とし、また、ランプ信号に係る速度が制御対象に係る最大速度の場合にはランプ信号の折れ時間を、制御指令値計算手段からの位置に係る制御指令値を最大速度で割った時間として求めるランプ折れ時間値決定手段と、
ランプ折れ時間値決定手段からの折れ時間に基づいてランプ信号を生成するとともに、移動物体への接近動作の開始時点での制御対象の作用端位置と移動物体との間の距離を基準とした移動物体の位置変化量をランプ信号に重畳することによって変調信号を生成する変調信号生成手段と、
変調信号生成手段からの変調信号及び時間軸伸縮用パラメータ値決定手段からの時間軸伸縮用パラメータ値に基づいて加減速曲線の山の高さや時間軸方向の幅を変化させることにより加減速曲線を生成して、該加減速曲線に係る制御信号を制御対象に送出する加減速パターン生成手段とを備えた
ことを特徴とする移動物体への接近及び／又は追従の制御に係るサーボ制御装置。
【請求項２】
請求項１に記載の移動物体への接近及び／又は追従の制御に係るサーボ制御装置において、
（１）加減速パターン生成手段が、制御対象に係るサーボ系の周波数特性を摸したフィルタ構成を有する仮想的なサーボ系を備えていること、
（２）（１）の仮想的なサーボ系は、その時定数又は時定数に係るパラメータについての基準値を設定することで規定される基準系を有すること、
（３）時間軸伸縮用パラメータ値決定手段からの時間軸伸縮用パラメータ値に基づいて時定数又は時定数に係るパラメータの基準値に対する変倍操作によって基準系の極配置を変化させて、時間軸伸縮用パラメータ値に対応する仮想的なサーボ系がその都度生成され、これに制御指令値計算手段からの制御指令値を入力した時の出力が加減速曲線として得られること、
を特徴とする移動物体への接近及び／又は追従の制御に係るサーボ制御装置。
【請求項３】
請求項１に記載の移動物体への接近及び／又は追従の制御に係るサーボ制御装置において、
制御対象の運動状態に関する情報と移動物体検出手段からの情報に基づいて、位置や速度及び／又は加速度の差が所定の範囲に収まったこと又は移動物体への接近動作に係る制御の開始指令が指令手段によって発せられた時点を起点にランプ信号の折れ時間とピーク速度到達時間との和に相当する時間か経過したことから、制御対象の作用端が移動物体を捕捉し又は作用端が移動物体に近接したことを判定する捕捉／近接判定手段と、
加減速パターン生成手段と制御対象との間に設けられ、捕捉／近接判定手段の信号を受けて、制御対象の作用端が移動物体を捕捉し又は作用端が移動物体に近接したことが判定された場合に、制御指令値計算手段又は変調信号生成手段の出力する移動物体の位置への制御指令値をそのまま制御対象に送出し、また、制御対象の作用端が移動物体を捕捉し又は作用端が移動物体に近接したことが判定されるまでの間は、変調信号生成手段の出力する変調信号が加減速パターン生成手段を介して制御対象に送出されるように切り換える切換手段とを備えている
ことを特徴とする移動物体への接近及び／又は追従の制御に係るサーボ制御装置。
【請求項４】
請求項１に記載の移動物体への接近及び／又は追従の制御に係るサーボ制御装置において、
指令手段が、制御対象の作用端による移動物体の捕捉又は制御対象の作用端の移動物体への接近の完了までに要する時間の長さを伸縮させるための指令をランプ折れ時間値決定手段に発した場合に、
ランプ折れ時間値決定手段が、伸縮前のランプ信号の折れ時間とピーク速度到達時間との和に対して、指令手段からの伸縮に係る比率を掛けることによって得られる時間からピーク速度到達時間を差し引くことによってランプ信号に係る新たな折れ時間を決定する
ことを特徴とする移動物体への接近及び／又は追従の制御に係るサーボ制御装置。
【請求項５】
その移動経路が予測できないような移動物体に対して制御対象の作用端を接近させたりあるいは移動物体の軌跡に追従させるための、移動物体への接近及び／又は追従の制御に係るサーボ制御方法において、
（１）先ず、移動物体への接近や追従制御の開始指令が発せられた場合に、移動物体の位置情報と、制御対象の作用端の位置情報とに基づいて、両者間の差をゼロにするために、制御対象の作用端への制御指令値を計算し、
（２）（１）の制御指令値と、制御対象に係る加減速曲線のピーク速度到達時間との関係を規定するとともに、制御指令値からピーク速度到達時間を決定し、該ピーク速度到達時間をその基準値で割ることによって無次元量として定義されるの時間軸伸縮用パラメータ値を決定し、
（３）（１）の制御指令値からランプ位置指令に係るランプ信号の折れ時間を求めるに際して、ランプ信号の傾斜つまり速度が制御対象に係る最大速度未満の場合にはランプ信号の折れ時間をピーク速度到達時間と同じ値とし、また、ランプ信号に係る速度が制御対象に係る最大速度の場合にはランプ信号の折れ時間を、制御指令値計算手段からの位置に係る制御指令値を最大速度で割った時間とし、
（４）（３）の折れ時間に基づいてランプ信号を生成するとともに、移動物体への接近動作の開始時点での制御対象の作用端位置と移動物体との間の距離を基準とした移動物体の位置変化量をランプ信号に重畳することによって変調信号を生成し、
（５）（４）の変調信号及び（２）の時間軸伸縮用パラメータ値に基づいて加減速曲線の山の高さや時間軸方向の幅を制御することにより加減速曲線を生成して、該加減速曲線に係る制御信号を制御対象に送出するようにした、
ことを特徴とする移動物体への接近及び／又は追従の制御に係るサーボ制御方法。
【請求項６】
請求項５に記載の移動物体への接近及び／又は追従の制御に係るサーボ制御方法において、
制御対象に係るサーボ系の周波数特性を摸したフィルタ構成を有する仮想的なサーボ系に関して、その時定数又は時定数に係るパラメータについての基準値を設定することで規定される基準系を用意しておき、
時間軸伸縮用パラメータ値に基づいて時定数又は時定数に係るパラメータの基準値に対する変倍操作によって基準系の極配置を変化させ、時間軸伸縮用パラメータ値に対応する仮想的なサーボ系を生成して、これに制御対象の作用端への制御指令値を入力した時の出力を加減速曲線として得るようにした
ことを特徴とする移動物体への接近及び／又は追従の制御に係るサーボ制御方法。
【請求項７】
請求項５に記載の移動物体への接近及び／又は追従の制御に係るサーボ制御方法において、
制御対象の作用端の運動状態を検出し、これらと移動物体の運動状態とを比較して位置や速度及び／又は加速度の差が所定の範囲に収まったこと又は移動物体への接近動作に係る制御の開始指令が発せられた時点を起点にランプ信号の折れ時間とピーク速度到達時間との和に相当する時間か経過したことから、制御対象の作用端が移動物体を捕捉し又は作用端が移動物体に近接したことを判定し、
制御対象の作用端が移動物体を捕捉し又は作用端が移動物体に近接したことが判定された場合には、移動物体の位置を目標値とする指令値をそのまま制御対象に送出し、また、制御対象の作用端が移動物体を捕捉し又は作用端が移動物体に近接したことが判定されるまでの間は、変調信号及びピーク速度到達時間若しくは時間軸伸縮用パラメータ値に基づいて生成される加減速曲線に従う制御信号を制御対象に送出するようにした
ことを特徴とする移動物体への接近及び／又は追従の制御に係るサーボ制御方法。
【請求項８】
請求項５に記載の移動物体への接近及び／又は追従の制御に係るサーボ制御方法において、
移動物体の位置情報と、移動物体への接近及び追従制御の開始指令を受けた時点における制御対象の作用端の位置情報とに基づいて、両者間の差をゼロにするために、制御対象の作用端への制御指令値を計算し、
該制御指令値からピーク速度到達時間及び時間軸伸縮用パラメータ値を決定した後、時間軸伸縮用パラメータ値を固定して加減速曲線を生成するようにした
ことを特徴とする移動物体への接近及び／又は追従の制御に係るサーボ制御方法。
【請求項９】
請求項５に記載の移動物体への接近及び／又は追従の制御に係るサーボ制御方法において、
移動物体の位置情報と、移動物体への接近及び追従制御の開始指令を受けた時点以後において変化する制御対象の作用端の位置情報とに基づいて、両者間の差をゼロにするために、制御対象の作用端への制御指令値を計算し、
該制御指令値の変化に応じて更新されるピーク速度到達時間及び時間軸伸縮用パラメータ値に基いて加減速曲線の時間軸方向の幅を制御するようにした
ことを特徴とする移動物体への接近及び／又は追従の制御に係るサーボ制御方法。
【請求項１０】
請求項５に記載の移動物体への接近及び／又は追従の制御に係るサーボ制御方法において、サーボ制御方法において、
制御対象の作用端による移動物体の捕捉又は制御対象の作用端の移動物体への接近の完了までに要する時間の長さを伸縮させるための指令が発せられた場合に、伸縮前のランプ信号の折れ時間とピーク速度到達時間との和に対して、伸縮に係る比率を掛けることによって得られる時間からピーク速度到達時間を差し引くことによってランプ信号に係る新たな折れ時間を決定するようにした
ことを特徴とする移動物体への接近及び／又は追従の制御に係るサーボ制御方法。
【請求項１１】
ロボットが、複数のアーム及び／又はツール搭載部と、アーム又はツール搭載部の駆動源及びその駆動回路を含む複数のサーボ系とから構成され、該ロボットの作用端を、その移動経路が予測できないような移動物体に対して接近させたりあるいは移動物体の軌跡に追従させる動作制御を行うロボット制御装置において、
各サーボ系を制御するためのサーボ制御手段が、
移動物体への接近動作や追従動作に係る制御の開始指令を発する指令手段と、
移動物体の運動状態を検出する移動物体検出手段と、
指令手段からの上記開始指令を受けた場合に、移動物体検出手段からの移動物体の位置情報と、ロボットの作用端の位置情報とに基づいて、両者間の差をゼロにするためにロボットの作用端への制御指令値を計算する制御指令値計算手段と、
制御指令値計算手段からの制御指令値と、サーボ系に係る加減速曲線のピーク速度到達時間との関係を規定するとともに、制御指令値からピーク速度到達時間を決定するピーク速度到達時間値決定手段と、
上記ピーク速度到達時間をその基準値で割ることによって無次元量として定義される時間軸伸縮用パラメータ値を決定する時間軸伸縮用パラメータ値決定手段と、
制御指令値計算手段からの制御指令値に基づいて位置指令に係るランプ信号の折れ時間を求める際に、ランプ信号の傾斜つまり速度がサーボ系に係る最大速度未満の場合にはランプ信号の折れ時間をピーク速度到達時間と同じ値とし、また、ランプ信号に係る速度がサーボ系に係る最大速度の場合にはランプ信号の折れ時間を、制御指令値計算手段からの位置に係る制御指令値を最大速度で割った時間とするランプ折れ時間値決定手段と、
ランプ折れ時間値決定手段からの折れ時間に基づいてランプ信号を生成するとともに、移動物体への接近動作の開始時点でのロボットの作用端位置と移動物体との間の距離を基準とした移動物体の位置変化量をランプ信号に重畳することによって変調信号を生成する変調信号生成手段と、
変調信号生成手段からの変調信号及び時間軸伸縮用パラメータ値決定手段からの時間軸伸縮用パラメータ値に基づいて加減速曲線の山の高さや時間軸方向の幅を変化させることにより加減速曲線を生成して、該加減速曲線に係る制御信号をサーボ系に送出する加減速パターン生成手段とを備えた
ことを特徴とするロボット制御装置。
【請求項１２】
ロボットが、複数のアーム及び／又はツール搭載部と、アーム又はツール搭載部の駆動源及びその駆動回路を含む複数のサーボ系とから構成され、該ロボットの作用端を、その移動経路が予測できないような移動物体に対して接近させたりあるいは移動物体の軌跡に追従させる動作制御に係るロボット制御方法において、
（１）先ず、移動物体への接近や追従制御の開始指令が発せられた場合に、移動物体の位置情報と、ロボットの作用端の位置情報とに基づいて、両者間の差をゼロにするために、ロボットの作用端への制御指令値を計算し、
（２）（１）の制御指令値と、サーボ系に係る加減速曲線のピーク速度到達時間との関係を規定するとともに、制御指令値からピーク速度到達時間を決定し、該ピーク速度到達時間をその基準値で割ることによって無次元量として定義されるの時間軸伸縮用パラメータ値を決定し、
（３）（１）の制御指令値からランプ位置指令に係るランプ信号の折れ時間を求めるに際して、ランプ信号の傾斜つまり速度がサーボ系に係る最大速度未満の場合にはランプ信号の折れ時間をピーク速度到達時間と同じ値とし、また、ランプ信号に係る速度がサーボ系に係る最大速度の場合にはランプ信号の折れ時間を、制御指令値計算手段からの位置に係る制御指令値を最大速度で割った時間とし、
（４）（３）の折れ時間に基づいてランプ信号を生成するとともに、移動物体への接近動作の開始時点でのロボットの作用端位置と移動物体との間の距離を基準とした移動物体の位置変化量をランプ信号に重畳することによって変調信号を生成し、
（５）（４）の変調信号及び（２）の時間軸伸縮用パラメータ値に基づいて加減速曲線の山の高さや時間軸方向の幅を制御することにより加減速曲線を生成して、該加減速曲線に係る制御信号をサーボ系に送出するようにした、
ことを特徴とするロボット制御方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、その移動経路が予測できないような対象物への接近や追従のための制御を行う移動物体への接近及び追従制御に係るサーボ制御装置及びサーボ制御方法並びにこれらを用いたロボット制御装置及びロボット制御方法に関する。
【０００２】
【従来の技術】
移動物体を掴んで別の場所に移送したり、あるいは、移動物体に追従してこれに付された識別情報を読み取る等の制御では、移動物体への接近のための制御や、その後に移動物体に追従するための制御が基本となる。例えば、ロボットの制御では、コンベア等によって搬送されてくるワークをロボットの作用端で捕捉したり、あるいはロボットの作用端をワークに追従させて移動させるような例を挙げることができる。
【０００３】
このような制御において、移動物体の軌跡が予め知られている場合には、当該軌跡を前もって制御装置に教示しておき、移動物体が予定場所を通過する前にロボットの作用端を予定場所に待機させておけば、移動物体の通過時にロボットの作用端で移動物体を捕捉したり、あるいは、移動物体の通過時以後にロボットの作用端を移動物体に追従させるように制御することができる。
【０００４】
【発明が解決しようとする課題】
しかしながら、上記の方法は、対象物の移動経路を予測することができない場合には使えないという問題がある。
【０００５】
そこで、このような場合には、制御対象の作用端の位置と移動物体の位置、そしてこれらの間の距離に係る情報を得て両者間の距離の差をゼロにするように制御すれば良いことになるが、制御対象の作用端を移動物体に接近させるためのアルゴリズムと、制御対象の作用端を静止物体又は静止位置へ移動させるためのアルゴリズムとが異なる場合には、制御が複雑になってしまうという不都合が生じる。
【０００６】
本発明は、対象物の移動の予測が不能な場合でも、対象物への接近及び追跡制御を行うとともに、該制御と、制御対象の作用端を静止物体又は静止位置へと移動させる制御との統一化を図ることを課題とする。
【０００７】
【課題を解決するための手段】
そこで、本発明は上記した課題を解決するために、下記の（１）乃至（５）に示す構成を有するものである。
【０００８】
（１）先ず、移動物体への接近や追従制御の開始指令が発せられた場合に、移動物体の位置情報と、制御対象の作用端の位置情報とに基づいて、両者間の差をゼロにするために、制御対象の作用端への制御指令値を計算する。そのために指令手段、移動物体検出手段、制御指令値計算手段を設ける。
【０００９】
（２）（１）の制御指令値と、制御対象に係る加減速曲線のピーク速度到達時間との関係を規定するとともに、制御指令値からピーク速度到達時間を決定し、該ピーク速度到達時間をその基準値で割ることによって無次元量として定義されるの時間軸伸縮用パラメータ値を決定する。そのためにピーク速度到達時間値決定手段と時間軸伸縮用パラメータ値決定手段を設ける。
【００１０】
（３）（１）の制御指令値からランプ位置指令に係るランプ信号の折れ時間を求めるに際して、ランプ信号の傾斜つまり速度が制御対象に係る最大速度未満の場合にはランプ信号の折れ時間をピーク速度到達時間と同じ値とし、また、ランプ信号に係る速度が制御対象に係る最大速度の場合にはランプ信号の折れ時間を、制御指令値計算手段からの位置に係る制御指令値を最大速度で割った時間とする。そのためにランプ折れ時間値決定手段を設ける。
【００１１】
（４）（３）の折れ時間に基づいてランプ信号を生成するとともに、移動物体への接近動作の開始時点での制御対象の作用端位置と移動物体との間の距離を基準とした移動物体の位置変化量をランプ信号に重畳することによって変調信号を生成するために変調信号生成手段を設ける。
【００１２】
（５）（４）の変調信号及び（２）の時間軸伸縮用パラメータ値に基づいて加減速曲線の山の高さや時間軸方向の幅を制御することにより加減速曲線を生成する加減速パターン生成手段を設け、該加減速曲線に係る制御信号を制御対象に送出する。
【００１３】
従って、本発明によれば、移動物体の運動状態を移動物体検出手段によって常時検出することによって予測不能な移動物体の動きを監視し、ランプ信号を基礎としてこれに移動物体の移動量に係る信号を重畳することにより変調信号を生成し、該変調信号に基づいて加減速曲線の山の高さを制御したり、時間軸伸縮用パラメータ値に基づいて加減速曲線の時間軸方向の幅を制御することで移動物体への接近や追従動作が実現される。そして、制御対象の作用端を静止物体又は静止位置へと移動させる場合には、変調信号の代わりに単にランプ信号に基づいて（つまり、変調信号における移動物体の移動量に係る信号分をゼロとする。）、加減速曲線の山の高さを制御したり、時間軸伸縮用パラメータ値に基づいて加減速曲線の時間軸方向の幅を制御すれば良い。
【００１４】
【発明の実施の形態】
以下に、本発明に係るサーボ制御装置及びサーボ制御方法について説明する。
先ず、本発明に係る「移動物体」とは、移動している物体をいうことは勿論、その途中に停止状態を含んで移動している場合を包含している。よって、移動物体が直線及び／又は曲線に沿って常時運動している場合だけに限られない。
【００１５】
そして、制御対象の作用端を移動物体に接近させたり、制御対象の作用端で移動物体を追いかける動作を以下では、「トラッキング動作」と呼ぶことにし、該トラッキング動作は「接近動作」や「追従動作」を含むものとする。
【００１６】
即ち、「接近動作」とは、制御対象の作用端が移動物体から所定の範囲内に入るように近づける動作であり、制御対象と移動物体についての位置関係でだけなく速度、加速度をも考慮したものである。正確には、制御対象の作用端の位置ベクトルを「Ｒ」、その速度ベクトルを「Ｒ(1)」（右上の添え字「(ｎ)」は、時間ｔに関するｎ階の微分を意味し、この略記法は以後も使用する。）、加速度ベクトルを「Ｒ(2)」とし、また、移動物体の位置ベクトルを「ｒ」、その速度ベクトルを「ｒ(1)」、加速度ベクトルを「ｒ(2)」とし、それぞれのベクトル差を「Δε0＝Ｒ−ｒ」、「Δε1＝Ｒ(1)−ｒ(1)」、「Δε2＝Ｒ(2)−ｒ(2)」で定義した時に、これらのベクトク差が所定の範囲内にある場合に制御対象の作用端が移動物体に「近接している」と言うことができる。
【００１７】
例えば、図１（ａ）に示すように、ベクトル差Δε0は、点Ｃで示す移動物体の位置と点Ｅで示す制御対象の位置との間の位置ベクトルの差であり、これが点Ｃを中心として半径「ε0」の球（又は円）内に入っていれば、位置に関して制御対象ｂの作用端が移動物体に近接していると判定を下すことができる。
【００１８】
このことは速度についても考えることができ、図１（ｂ）に示すように、点Ｃ′で示す移動物体の速度と点Ｅ′で示す制御対象の作用端の速度との間のベクトル差Δε１が、点Ｃ′を中心として半径「ε1」の球（又は円）内に入っていれば、速度に関して制御対象の作用端が移動物体に近接していると判定を下すことができる。
【００１９】
また、加速度については、図１（ｃ）に示すように、点Ｃ′′で示す移動物体の加速度と点Ｅ′′で示す制御対象の作用端の加速度との間のベクトル差Δε2が、点Ｃ′′を中心として半径「ε2」の球（又は円）内に入っていれば、加速度に関して制御対象が移動物体に近接していると判定を下すことができる。
【００２０】
このように、ベクトルの組み（Δε0，Δε1，Δε2）を新たなベクトルεとして定義すれば、その成分がそれぞれの所定範囲内にある場合に制御対象が移動物体に接近していると言える。換言すれば、ベクトルεは制御対象がその運動状態において移動物体の運動状態にどれだけ近づいているか示すものであり、ベクトルεの各成分について指定される閾値（上記ε0，ε1，ε2）の如何によって近さの判定が粗くなったり又は詳細になったりするという意味で、判定精度を表すものである（以下、ベクトルεのことを「精度ベクトル」と呼ぶ。）
しかして、「接近動作」とは、精度ベクトルεの各成分が所定の範囲内にある状態まで制御対象を移動物体に近づける動作をいう。
【００２１】
また、「追従動作」とは、精度ベクトルεの各成分が所定の範囲内にある状態で制御対象を移動物体を追いかける動作であり、追従動作から逸脱した時、つまり、精度ベクトルεのいづれか成分が所定の範囲内から外れてしまった場合を、「トラッキングアウト」ということにする。
【００２２】
このトラッキングアウトと対をなす「トラッキングイン」とは、トラッキング動作の開始後に接近動作が完了した状態をいい、トラッキング動作の開始時点からトラッキングインまでに要する時間を「トラッキングイン時間」といい、「Ｔtr」で表すことにする。
【００２３】
以上で用語の定義が明らかとなったので、本発明に係るサーボ制御装置の構成について説明する。
【００２４】
図２に示すように、サーボ制御装置１は、指令手段２、移動物体検出手段３、トラッキング制御手段４（制御指令値計算手段５、ＴP（ピーク速度到達時間）値決定手段６、Ｔf（ランプ信号の折れ時間）決定手段７、変調信号生成手段８、α（時間軸伸縮用パラメータ）値決定手段９、捕捉／接近判定手段１０からなる。）、加減速パターン生成手段１１、切換手段１２（図ではスイッチの記号で示す。）から構成されている。
【００２５】
本発明に係る制御対象は、移動物体への接近動作や追従動作が可能な作用端を有するものに限られるが、該作用端によって必ずしも移動物体を捕まえたり、移動物体に対して何等かの影響を及ぼす必要はない。移動物体への接近動作や追従動作は、作用端を移動物体に対して何ら接触させることなく行い得る動作だからである。尚、図２では、制御対象１３を「サーボ系」に特定しているが、これは駆動源及び／又は駆動機構の他、サーボ回路等を含む広い系を意味しており、単一の部品だけから成る系や、これに付随する制御部品を含む系を指す。
【００２６】
指令手段２は、移動物体へのトラッキング動作についての開始指令を発するものであり、該指令は制御指令値計算手段５に送られる。
【００２７】
加減速パターン生成手段１１は、何等かの制御指令に応じて加減速制御に係る制御パターンを生成するものである。尚、ここにいう「加減速パターン」とは、加減速曲線（あるいは加減速指令曲線）によって直接に記述される状態量（つまり速度又は角速度）は勿論、該状態量の時間微分や積分値の変化等を含む広義の概念である。つまり、位置や角度を示す関数の時間による１階微分で定義される速度関数又は角速度関数は、その制御において加速期間や減速期間（時には定速期間）を含んでおり、これをグラフ化したものが加減速曲線に他ならないが、「加減速パターン」という時には、速度や角速度の他、速度又は角速度に関連して時間とともに変化する量（位置や加速度等）を含むものとする。
【００２８】
加減速パターンの生成法には、例えば、下記に示す２通りの方法を挙げることができる。
【００２９】
（１）関数計算による方法（図３参照。）。
【００３０】
現実の制御対象の応答特性が、理論的に明確に解析されている場合に、制御変数を関数式として表現することができれば、加減速パターンの生成対象となる量を予め数式によって用意することができる。
【００３１】
例えば、速度を例にすると、図３に示す速度の基準関数ΩN（ｔ）に「スケーリング操作」を施すことによって加減速曲線を生成することができる。尚、「スケーリング操作」とは関数値そのものの変倍操作と関数の時間軸方向の伸縮操作を含む操作である。即ち、基準関数ΩN（ｔ）に対して制御対象についての総移動量Δθを掛けることによって図にΩv（ｔ）で示すように山の高さを変化させたり、あるいは、ピーク速度到達時間ＴPnの基準関数ΩN（ｔ）に対してＴPnをα倍することによって図にΩt（ｔ）で示すように時間軸方向の伸縮の度合を変化させる操作を基本として、両操作を組み合わせることによって図にΩv*t（ｔ）に示すような曲線を得ることができる。尚、時間軸伸縮用パラメータαの値についてはΔθとの関係において決定する必要があるが、その説明については後述する。
【００３２】
このように、制御変数に係る基準関数（又は関数群）を予め決定しておけば、これらにスケーリング操作を施すことによって加減速パターンに係る関数を容易に得ることができ、基準関数の関数値の計算とこれに四則演算を組みあわせた計算レベルで済むことになる。尚、加減速曲線を構成する加速部分と減速部分とが時間軸について対称性（線対称性及び／又は回転対称性）を有する場合には、加速部分又は減速部分に係る基準関数だけを用意すれば良い。何故なら、どちらか一方の部分に係る基準関数に対して時間軸に係る操作を施せば他方の部分に係る関数を容易に求めることができるからである。
【００３３】
（２）仮想サーボ系を用いる方法（図４参照。）。
【００３４】
ここにいう「仮想サーボ系」とは、現実のサーボ系に対応した構成をソフトウェア処理によって模擬的に構築して制御装置内に用意される系であり、これを計算手段として用いることによって加減速パターンの生成や各種の推定量の算出に利用することができる。
【００３５】
例えば、モータの制御回路には、モータの制御に直接に関係する回路部分や制御の安定化のための動的補償に係る部分等がサーボ系内に設けられるが、これらに対応した構成を有する仮想サーボ系（以下、「仮想モータ系」という。）を、ソフトウェア処理による内部モデルとして制御装置内に用意すれば、仮想モータ系に対して指令を与えた時の応答出力を得て、これをそのまま現実のサーボ系の制御に利用することができる。
【００３６】
また、サーボ系の構成が比較的簡単な場合、即ち、サーボ系の応答特性が解析式によって表現することができ、計算が簡単な場合には上記（１）のように関数計算による方法が有効であるが、一般には、サーボ系の応答特性が常に解析式によって容易に表わされるとは限らないので、現実のサーボ系を摸した仮想サーボ系を導入することは、実際的な見地から有用である。
【００３７】
図４は、制御対象をモータ系（あるいはモータ制御系）に特定した場合の現実のモータ系と仮想モータ系の一例を示すものである。
【００３８】
図４（ａ）は現実のモータ系の一例を簡略化してシグナルフロー線図で示したものであり、図中の１点鎖線の左側がソフトウェア処理又は回路によって構築される部分であり、１点鎖線の右側がモータや機械系の部分である。また、図４  （ｂ）は図４（ａ）に対応した仮想モータ系を示すシグナルフロー線図であり、全てソフトウェア処理により構築することができる。
【００３９】
尚、図中の記号の意味を表１にまとめて示す。
【００４０】
【表１】
0003648812.tif 000002
【００４１】
今、上記仮想モータ系にアナログ近似（つまり、１−ｚ-1≒Ｔs・ｓ、（１＋ｚ-1）／（２・（１−ｚ-1））≒１／（Ｔs・ｓ））を施すことによって下式［数１］に示すようなオープン特性に係る伝達関数（これ「ＧOP（ｓ）」と記す。）が求まり、この式から位置指令入力ΘIN（ｓ）に対する位置出力ΘOUT（ｓ）の比から閉ループ伝達関数が下式［数２］に示すように得られる。
【００４２】
【数１】
0003648812.tif 000003
【００４３】
【数２】
0003648812.tif 000004
【００４４】
尚、［数２］式におけるＴi（ｉ＝１、２、３）はループの時定数に相当する量であり、「−１／Ｔi」はループの極を示す。つまり、［数２］式の第２式の分母であるｓについての３次式を因数分解したものが［数２］式の第３式の分母であり、両者を比較することによってＴiとサーボパラメータとの間には下式のような関係があることが分かる。
【００４５】
【数３】
0003648812.tif 000005
【００４６】
尚、この例では、［数２］式から明らかなように仮想モータ系が３次のロ−パスフィルタと等価な構成を有するが、仮想サーボ系は、一般にはｎ（ｎは自然数。）次のフィルタと等価な構成を有することに注意する。
【００４７】
上記の仮想モータ系に各種の位置入力（これを「θREF」とする。）を与えた時の出力によって加減速パターンを得ることができ、本発明ではランプ信号を基本信号としている。
【００４８】
図５（ａ）に示すように、θREFとしてランプ信号を用いる場合に（ランプ信号の折れ時間を「Ｔf」とする。）、位置、速度、加速度の出力をそれぞれ「θO（ｔ）」、「θO(1)（ｔ）」、「θO(2)（ｔ）」とすると、これらは下式のようになる。
【００４９】
【数４】
0003648812.tif 000006
【００５０】
尚、ピーク速度到達時間ＴPは、加速度がゼロになる時点として下式［数５］に示すように決まり、これはΔθやＴfに無関係な量（Ｔi（ｉ＝１、２、３）のみに依存する。）である。
【００５１】
【数５】
0003648812.tif 000007
【００５２】
図５（ｂ）にθO（ｔ）とθO(1)（ｔ）の曲線の一例を併せて示す。
【００５３】
［数４］式の導出のためには、ランプ信号についてラプラス変換を施したものに［数２］式の右辺を掛けてから逆ラプラス変換を行えば良いが、それらの計算の過程は重要でないので省略する。
【００５４】
尚、図５（ａ）に示すランプ信号の傾斜部が時間軸ｔに対してなす角度φ（速度に対応する。）を次第に大きくしていくと遂にはφ＝９０°となってステップ信号となり、その意味でランプ信号はステップ信号をも包含していることに注意する。
【００５５】
ところで、［数５］式において、時間の次元をもつ量（Ｔi、Ｔf）をすべてα倍する操作を考えると、下式のようになる。
【００５６】
【数６】
0003648812.tif 000008
【００５７】
上式によってＴiやＴfをα倍しても［数５］式と同様の関係が成り立ち、新たなピーク速度到達時間がα・ＴPとなる。つまり、Ｔiをα倍することは［数３］式におけるサーボパラメータを下式［数７］のように変換すること等価であり（但し、Ｔsについては固定）、仮想モータ系の周波数特性の形を変えずにループの極配置を変更することである。以下では、この変換のことを「α変換」と呼ぶことにする。
【００５８】
【数７】
0003648812.tif 000009
【００５９】
α変換によって、［数４］式のθO（ｔ）、θO(1)（ｔ）、θO(2)（ｔ）がどのように変換するかを知るのは容易であり、これらは下式のような変換則に従う。
【００６０】
【数８】
0003648812.tif 000010
【００６１】
尚、［数４］式の第３式を時間によりもう一回微分したものがゼロとなる時点によって加速度が正値のピーク値に達する時間や加速度が負値のピーク値に達する時間を求めることができ、これらの時間をα倍することはα変換によって加速度が下式のようになることと等価である。
【００６２】
【数９】
0003648812.tif 000011
【００６３】
よって、加速度の大きさはΩ（＝Δθ／Ｔf）やαの値によって制御することができる。尚、このような加速度に係る制御は、モータ系のトルク制限、つまり、モータ電流を決定する加速度が過大な値にならないようにするのに必要となる。  以上のように、α値を何等かの方法によって決めれば、α変換によって仮想モータ系の周波数特性に係る極配置を制御することができることが明らかとなったが、α値が比の値であることから分かるようにその基準が必要となる。
【００６４】
そこで、仮想モータ系の基準系を、そのピーク速度到達時間値「ＴPMIN」を用いて定義すると、該基準系は、図６に示すように、サーボパラメータＫiMIN、ＫPMIN、ＫMINとＴPMINとの組み[ＫiMIN，ＫPMIN，ＫMIN，ＴPMIN]によってその特性を記述することができる。尚、基準系のサーボパラメータＫiMIN、ＫPMIN、ＫMINは、現実のモータ系に係る時定数又はサーボパラメータ値に合せて決められる。
【００６５】
この基準系の導入によって、αは下式のように定義される。
【００６６】
【数１０】
0003648812.tif 000012
【００６７】
つまり、何等かの方法で決定されるＴP値からαを求め、図６に示すように、基準系に対してα変換を施すことによって仮想モータ系が得られ（[Ｋi＝ＫiMIN／α2，ＫP＝α・ＫPMIN，Ｋ＝α・ＫMIN，ＴP＝α・ＴPMIN]によりその特性が特定される。）、これに位置指令θREFを入力すれば、その出力θOとしてピーク速度到達時間ＴPの加減速パターンを得ることができる。尚、ロボットのように複数の駆動軸によって構成される系では、一般には各サーボ系に対応した仮想サーボ系が用いられる。
【００６８】
さて、トラッキング動作に係る加減速パターンの生成について説明するに先だって、制御対象の作用端を静止物体又は定位置へと移動させる動作（以下、「定点間移動」という。）について説明する。その理由はこの動作が最終的にトラッキング動作の中に吸収されるからであり、また、移動物体への接近動作に比べて簡単であるからである。尚、定点間移動の例としては、ロボット制御において始点と終点の途中経路を指定されずに２点間でロボットの作用端を移動させるＰＴＰ（Ｐｏｉｎｔ  Ｔｏ  Ｐｏｉｎｔ）動作等を挙げることができる。
【００６９】
図７は定点間移動に係る加減速パターンの生成の仕方を概念的に示すものであり、上記の仮想サーボ系を用いた例を示している。
【００７０】
図中の、「θREF」はランプ指令信号であり、これが仮想モータ系の位置指令に係るノードに与えられた結果、加減速パターン、つまり、速度出力θO(1)や位置出力θOが得られることになる。
【００７１】
ランプ指令信号θREFは、その折れ時間Ｔfに達するまでの期間（０≦ｔ≦Ｔf）において一定の傾きの傾斜部を有し、ｔ＞Ｔfにおいて一定値Δθとなり、これが速度θO(1)に係るピーク速度到達時間ＴPだけ持続する信号である。よって、傾斜部の傾きはΔθ／Ｔfに等しい。
【００７２】
また、速度θO(1)は、その加速期間の長さと減速期間の長さがいづれもピーク速度到達時間ＴPに等しくなっており、減速部の形状は加速部の形状に対して対称性を有しており、両者は時間軸についての線対称化操作及び点対称化（回転）操作によってお互いに一致させることができる。尚、このような時間軸に係る対称性は、制御対象の加速度−速度線図上の特性についての対称性に起因しており、よって、一般には加速期間の長さと減速期間の長さとが一致するとは限らないことに注意を要する。また、位置θOや速度θO(1)の形状についての具体例については上記［数４］式を参照されたい。
【００７３】
ところで、上記したように加減速パターンの生成にあたって、α値の決定のためにはＴP値を決定する必要があり、このために総移動量ΔθとＴPとを予め関係付けておいてΔθからＴPを決定する方法を用いることができる。
【００７４】
例えば、図８に示すように、横軸にΔθをとり、縦軸にＴPをとると、ＴP値の下限値及び上限値にそれぞれ対応した点ＭＩＮ（ΔθMIN，ＴPMIN）、点ＭＡＸ（ΔθMAX，ＴPMAX）が制御対象に固有の点として決まる。つまり、下限値ＴPMINは、制御対象の周波数特性によって決まる値であり、例えば、上述した［数１０］式の基準値ＴPMINをこの下限値に一致させることができる。また、上限値ＴPMAXは制御対象の最大速度によって規定される。尚、ピーク速度到達時間の下限や上限を考慮して加減速曲線を生成する理由は、制御対象の能力を越える動作を禁止することで、過負荷や故障等が起きるのを防ぐためであり、これによって制御対象に対して負担のかからない加減速制御を行うことができる。
【００７５】
上記点ＭＩＮと点ＭＡＸとの間をどのような形状の線で結ぶかについては、何を評価関数にするかによって異なるが、その具体的な説明については本発明の要旨に直接関係がないので省略する。尚、図では、点ＭＩＮと点ＭＩＮとの間を直線で結んだ特性となっており、０≦Δθ≦ΔθMINの範囲ではＴP＝ＴPMIN、ΔθMIN≦Δθ≦ΔθMAXの範囲では直線の傾斜に応じたＴP値（但し、ＴPMIN≦ＴP≦ＴPMAX）、Δθ≧ΔθMAXの範囲ではＴP＝ＴPMAXがそれぞれ得られることになる。
【００７６】
また、この例では一のΔθに対してＴP値が決まるが、制御対象が複数の駆動系から構成されている場合には、各駆動軸に係るΔθi（ｉ＝１、２、・・・）のそれぞれに対応するＴPi（ｉ＝１、２、・・・）が決まり、場合によってそれらの中から選択する操作が必要となる（例えば、最大のものを選ぶ等。）。
【００７７】
以上のようにＴP値が求まると、［数１０］式に示すように、ＴP値をその基準値ＴPMINで割ることによってα値を決定することができる。
【００７８】
ランプ信号の折れ時間Ｔfの決定については、ランプ信号の傾斜部の傾きΩが制御対象の最高速（これを「ΩMAX」とする。）未満の場合にはＴf＝ＴPとし、Ω＝ΩMAXの場合には、「Δθ／Ｔf＝ΩMAX」から決まるＴf値とする。これを数式で表すと、下式のようにまとめることができる。
【００７９】
【数１１】
0003648812.tif 000013
【００８０】
従って、Ｔf値やＴP値の決定にあたっては、図９（ａ）に示すようにθ−ｔ線図とΔθ−ＴP線図とを重ね合せたグラフ図を用いると便利である。即ち、横軸に時間ｔをとり縦軸にθをとってランプ信号を示すグラフ図と、横軸にＴPをとり縦軸にΔθをとって両者の関係を規定する特性線を示すグラフ図とを併せたものが図９（ａ）である。
【００８１】
図中に太線で示す右上りの半直線Ｌは、Ω＝ΩMAX、つまり制御対象に許される最大速度を示す特性線であり、ランプ信号の傾斜角φは半直線の傾斜角とされる。つまり、ランプの傾斜部は、半直線Ｌ上か又はその下側に位置するように規制される。
【００８２】
図では移動量がΔθa、Δθb、Δθc（Δθa＜Δθb＜Δθc）の３つの値をとる場合について示しており、Δθaの場合には、これに対応するＴP値がＴPMINであり、ランプ信号の傾斜部の傾きが半直線Ｌの傾きより小さいことから明らかなようにＴf＝ＴPMINである。また、Δθbの場合には、これに対応するＴP値がＴP＝ＴP（Δθb）＝ＴPbであり、ランプ信号の傾斜部の傾きが半直線Ｌの傾きより小さいことから明らかなようにＴf＝ＴPbである。そして、Δθcの場合には、これに対応するＴP値がＴP＝ＴPMAXであり、ランプ信号の傾斜部の傾きが半直線Ｌの傾きに等しいので、Ｔf＝Δθc／ΩMAXとなり、Ｔf≠ＴPである。
【００８３】
よって、Δθ＝Δθaの場合のランプ信号は、図に点Ｏ（０，０）−点Ｐa（ＴPMIN，Δθa）−点Ｐa′（２・ＴPMIN，Δθa）に示すようになり、また、Δθ＝Δθbの場合のランプ信号は、図に点Ｏ（０，０）−点Ｐb（ＴPb，Δθb）−点Ｐb′（・ＴPb，Δθb）に示すようになり、そして、Δθ＝ΔθCの場合のランプ信号は、図に点Ｏ（０，０）−点ＰC（Ｔf（＝Δθc／ΩMAX），ΔθC）−点ＰC′（Ｔf（＝Δθc／ΩMAX）＋ＴPMAX，ΔθC）に示すようになる。また、Δθa、Δθb、Δθcのそれぞれの場合の加減速曲線を示すと図９（ｂ）のようになり、ΔθaやΔθbの場合には、加減速曲線が加速部と減速部とからなり、Δθcの場合には、加減速曲線が加速部とΩ＝ΩMAXの定速部と減速部とからなる。図示するようにこれらの加減速曲線は減速期間においてゆっくりと停止に近づいていき、この事は定点間移動における滑らかな停止制御を意味するが、これがトラッキング動作においては、制御対象の作用端を移動物体に対して忍び寄るように接近させる制御につながることが後に明らかとなる。
【００８４】
尚、上記の説明は制御対象が一の駆動軸を有する構成であるとしたが、制御系が複数の駆動系から構成されている場合に、上記したＴP値やＴf値の決定方法を拡張することは容易である。
【００８５】
即ち、ｉ（＝１、２、・・・、Ｍ）番目の駆動軸について、ＴP値を「ＴPi」（＝１、２、・・・、Ｍ）とし、Ｔf値を「Ｔfi」（＝１、２、・・・、Ｍ）とするとき、これらの最大値を各駆動軸に共通のＴP値、Ｔf値として採用すれば良く、数式で表すと下式のようになる。
【００８６】
【数１２】
0003648812.tif 000014
【００８７】
尚、上式において「ｍａｘ（Ｘ１、Ｘ２、・・・）」は、Ｘ１、Ｘ２、・・・のうち小さい方を選び出すための関数である。
【００８８】
例えば、制御対象が２軸の構成となっている場合のＴP値やＴf値の決定法を示すと図１０のようになる。尚、図１０は図９と同様に、横軸に時間ｔをとり縦軸にθをとってランプ信号を示すグラフ図と、横軸にＴPをとり縦軸にΔθをとって両者の関係を規定する特性線を示すグラフ図とを併せて示したものであり、図中に太線で示す右上りの半直線Ｌ１は、第１軸に係る最大速度（これを「Ω1MAX」とする。）を示す特性線であり、また、これより傾きの大きな半直線Ｌ２は、第２軸に係る最大速度（これを「Ω2MAX」とする。）を示す特性線である。
【００８９】
Δθ−ＴP特性は軸毎に異なっており、図１０に示す折れ線Ｎ１（点ＭＩＮ１（ＴPMIN1，ΔθMIN1）、点ＭＡＸ１（ＴPMAX1，ΔθMAX1）は上下限をそれぞれ示す。）が第１軸に係る特性を示し、折れ線Ｎ２（点ＭＩＮ２（ＴPMIN2，ΔθMIN2）、点ＭＡＸ２（ＴPMAX2，ΔθMAX2）は上下限をそれぞれ示すが第２軸に係る特性を示している。尚、この例では、ΔθMIN2＝ΔθMIN1、ΔθMAX2＜ΔθMAX1、ＴPMIN2＜ＴPMIN1＜ＴPMAX2＜ＴPMAX1である。
【００９０】
第１軸に関する移動量を「Δθ1」とし、第２軸に関する移動量を「Δθ2」とすると、ＴP値については、図１０に示す例ではΔθ1に対応するＴP値（これを「ＴP1」とする。）と、Δθ2に対応するＴP＝ＴPMAX2とを比較することによって後者が選ばれ、また、Ｔf値については、第１軸に係るＴf1がＴP1に等しく、第２軸に係るＴf2がΔθ2／ΩMAX2となり、両者の比較からＴf2が選ばれることになり、下式のようになる。
【００９１】
【数１３】
0003648812.tif 000015
【００９２】
よって、第１軸に係るランプ信号は図１０の線分Ｏ−Ｐ１−Ｐ１′に示すようになり、また、第２軸に係るランプ信号は図１０の線分Ｏ−Ｐ２−Ｐ２′に示すようになる。尚、点Ｏは時間軸の原点（０，０）であり、点Ｐ１（Ｔf2，Δθ1）、点Ｐ１′（Ｔf2＋ＴPMAX2，Δθ1）、点Ｐ２（Ｔf2，Δθ2）、点Ｐ２′（Ｔf2＋ＴPMAX2，Δθ2）である。
【００９３】
このようにＴP値やＴf値の決定法を多数の駆動軸からなる制御対象に関して一般化することが容易である。
【００９４】
ところで、以上の説明ではランプ信号の傾きが制御対象に係る最大速度に対応する傾きを越えてはいけないという要請が常に満たされるものと仮定したが、実際の制御においては、このための何等かの手段が必要となる。
【００９５】
図１１は横軸に時間ｔをとり、縦軸に位置（又は位相角）θをとったものであり、特性線Ｌの傾きが制御対象の最大速度ΩMAXに対応する。
【００９６】
今、期間０≦ｔ≦ｔ１におけるランプ信号の時間軸に対する傾斜角をφとし、期間ｔ１≦ｔ≦ｔ２においてランプ信号が増分Δφ１をもって上向きに折れ曲がった後、期間ｔ２≦ｔにおいてランプ信号がさらに増分Δφ２をもって上向きに折れ曲がった場合を想定すると、傾斜角の増分Δφ１、Δφ２は制御対象に係る加速度に対応するものであり、これらは制御対象の最大加速度（これを「θMAX(2)」とする。）に対応する角度を越えてはいけないものである。
【００９７】
即ち、傾斜角の増分に相当する加速度に対しては制御対象の最大加速度以下となるように制限する必要があるが、この制限だけでは、増分Δφの蓄積によって傾斜角の総和が特性線Ｌの傾きを越えてしまう場合が生じ得るので、ランプ信号の傾斜、つまり速度についても制限が必要となる。
【００９８】
そこで、図１２に示すように、加減速パターンの生成にあって仮想サーボ系を用いる場合には、指令手段２の送出する原信号Ｓに対して時間による１階の微分演算要素「ｄ／ｄｔ」を２つ直列に設けて速度θ(1)や加速度θ(2)を求めて、これらを比較演算要素「ＣＭＰ」においてそれぞれの最大値ΩMAX、θMAX(2)と比較してエラー信号を指令手段２にフィードバックし、原信号Ｓに補正を加えて信号Ｓ′（破線で示す。）を得るようにすれば、信号Ｓ′に係る速度θ(1)や加速度θ(2)がそれらの最大値を越えることがない。尚、このような議論は関数計算による加減速パターンの生成法に対しても容易に適用することができることは明らかである（微分演算要素とは関数の微分計算であり、比較演算要素とは差を求める計算に他ならないからである。）。
【００９９】
以上に説明した定点間移動に係る制御の手順をまとめると以下のようになる。
（１）制御対象の作用端を目的位置へと移動させるための制御指令値の算出。つまり、目標位置に係る情報と、制御対象の作用端の位置情報とに基づいて、両者間の差をゼロにするための制御指令値（Δθ）を計算する。
【０１００】
（２）加減速曲線に係るピーク速度到達時間（ＴP））の決定。
【０１０１】
上記（１）の制御指令値と加減速曲線のピーク速度到達時間との関係を図８に示すように予め規定しておき、制御指令値に対応するピーク速度到達時間を決定する。
【０１０２】
（３）ランプ信号の折れ時間（Ｔf）の決定。
【０１０３】
即ち、ランプ信号に係る速度が制御対象に係る最大速度未満の場合にはランプ信号の折れ時間をピーク速度到達時間と同じ値とし、また、ランプ信号に係る速度が制御対象に係る最大速度の場合には、（１）の制御指令値を最大速度で割った時間をランプ信号の折れ時間とする。
【０１０４】
（４）時間軸伸縮用パラメータ値の決定。
【０１０５】
ピーク速度到達時間をその基準値で割ることによって無次元量として定義される時間軸伸縮用パラメータαの値を決定する。
【０１０６】
（５）ランプ信号の生成。
【０１０７】
即ち、（１）の制御指令値や（３）の折れ時間に基づいてランプ信号を生成する。尚、場合によっては、速度や加速度については上述したような最大値による制限を考慮してランプ信号の傾斜を制御する必要が生じる。
【０１０８】
（６）加減速パターンの生成。
【０１０９】
つまり、（５）のランプ信号と、（４）の時間軸伸縮用パラメータに基づいて加減速曲線の山の高さや時間軸方向の幅を制御することにより加減速曲線を生成して、該加減速曲線に係る制御信号を制御対象に送出する。
【０１１０】
尚、上記（４）の手順は（２）の手順の直後に位置させても良い。
【０１１１】
次に、本題であるトラッキング動作の説明に移る。
【０１１２】
先ず、トラッキング動作からみた場合の上記定点間移動が停止物体へのトラッキング動作と等価であることについて説明する。
【０１１３】
制御対象が一の駆動軸を有する場合を考え、トラッキング動作の開始時点における制御対象の作用端の位置と移動物体の位置の間の距離差を「ΔθS」とし、移動物体の位置を時間ｔの関数として「θT（ｔ）」とし、また、ΔθS／Ｔfの傾斜を有するランプ信号の傾斜部を「θrmp（ｔ）」、ΔθSを基準とした移動物体の位置の変化分（つまり、「θT（ｔ）−ΔθS」）を「θ〜（ｔ）」、これをランプ信号に重畳した信号（つまり、「θrmp（ｔ）＋θ〜（ｔ）」であり、以下、「ランプ変調信号」という。）」を「θ〜rmp（ｔ）」と定義する。尚、これらを表にまとめたものが下表である。
【０１１４】
【表２】
0003648812.tif 000016
【０１１５】
図１３は上記に定義した諸量の関係を図示したものであり、図中の点Ｑにおいて移動物体の位置と制御対象の作用端が一致し、その時点がｔ＝Ｔfである。
【０１１６】
ここで、新たに導入されたランプ変調信号θ〜rmp（ｔ）が上記した定点間移動に係るランプ信号θrmp（ｔ）とどのような関係を有するかを理解するには、移動物体が静止している場合を想定すれば良く、この場合には「θT（ｔ）＝ΔθS」から分かるように「θ〜（ｔ）＝０」より「θ〜rmp（ｔ）＝θrmp（ｔ）」となってランプ信号そのものとなる。つまり、ランプ変調信号θ〜rmpは移動物体の位置変化を取り入れることによってランプ信号θrmpを拡張した信号であることが分かる。尚、ランプ変調信号θ〜rmpについて注意すべき点は期間ｔ≧Ｔfにおいてθ〜rmp（ｔ）＝θT（ｔ）とされる点である。
【０１１７】
ランプ変調信号θ〜rmpに関する諸関係を数式で表すと下式のようになる。
【０１１８】
【数１４】
0003648812.tif 000017
【０１１９】
上記のようにランプ変調信号θ〜rmpはランプ信号θrmpを含んでおり、ランプ信号を拡張したものとなっていることから、上記定点間移動に係るＴfとトラッキング動作に係るＴfとが実は同じであることを示すことができる。
【０１２０】
即ち、定点間移動ではランプ信号θrmpが下式のように表されるので、制御対象の作用端の位置がΔθSに示す位置に一致する時間は明らかにｔ＝Ｔfである（［数１５］式の第１式の右辺と第２式の右辺とを等しいとすれば良い。）。
【０１２１】
【数１５】
0003648812.tif 000018
【０１２２】
他方、トラッキング動作の場合には、ランプ変調信号θ〜rmpに関する［数１４］式において第３式の右辺がθTに等しいとおいた下式からやはりｔ＝Ｔfが得られる。
【０１２３】
【数１６】
0003648812.tif 000019
【０１２４】
また、ｔ＝Ｔfの時点において、制御対象の作用端と移動物体との間の相対速度（これは作用端と移動物体との衝突速度を「θREL(1)」とする。）を求めてみると、定点間移動の場合には移動物体が静止していると考えれば良いので、下式のようにθREL(1)はランプ信号θrmpの傾きに等しい。
【０１２５】
【数１７】
0003648812.tif 000020
【０１２６】
他方、トラッキング動作の場合には、下式のように計算することができ、やはりθREL(1)はランプ信号θrmpの傾きに等しい。尚、制御対象の運動が１次元であるため、相対速度の向きは正負によって表されることに注意すると、［数１７］式と［数１８］式とは相対速度の大きさ及び符号を含めて一致している。
【０１２７】
【数１８】
0003648812.tif 000021
【０１２８】
以上のような、定点間移動とトラッキング動作との比較からＴfや相対速度の一致が偶然ではないことは、ランプ変調信号θ〜rmpがランプ信号θrmpを含んでこれを拡張したものであること、換言すれば、定点間移動とは移動物体が静止している場合にその静止位置に制御対象の作用端を移動させるトラッキング動作に他ならないことから理解することができる。
【０１２９】
よって、上記した定点間移動に係る制御においてランプ信号をランプ変調信号に置換すれば、定点間移動に係る加減速曲線の制御性がトラッキング動作においても同様に実現されることが分かる。例えば、図９（ｂ）に示すような定点間移動における滑らかな停止制御は、トラッキング動作において、制御対象の作用端と移動物体との間の相対速度を徐々にゼロに近づけていく制御（つまり、移動物体から見ると移動物体への接近時に、作用端がゆっくりと移動物体に近づいてくる。）に相当するため、これによって作用端が勢い良く移動物体に衝突するといった不都合を防止することができる。
【０１３０】
尚、上記の説明では制御対象を一の駆動軸のみの系に特定したが、制御対象が複数の駆動軸を有する場合にも上記の議論をそのまま一般化することができる。何故なら、各駆動軸についてランプ変調信号がそれぞれに独立に定義することができ、それらについて定点間移動とトラッキング動作との間でＴf値や相対速度の一致を示すことができるからである。
【０１３１】
しかして、トラッキング動作に係る制御は、上記した定点間移動に係る制御を含むものとなるが、ランプ変調信号のうちランプ信号成分の生成に係るアルゴリズムについては、定点間移動の場合のアルゴリズムを踏襲することができる。
【０１３２】
例えば、ΔθSからＴP値を決定するには、ΔθS−ＴP特性を予め規定しておき、ΔθSと対応するピーク速度到達時間ＴPを決定すれば良く、この処理は図２においてＴP値決定手段６が担当する。但し、その際、ピーク速度到達時間ＴPが制御対象の性能を越える加減速曲線が生成されないように制御対象の能力に基づいてピーク速度到達時間ＴPに係る上限値（制御対象の最高速度により規定される。）や下限値（制御対象の周波数特性によって規定される。）を規定する必要があることは勿論である。
【０１３３】
尚、制御対象の運動が１次元の場合には、ΔθSは移動物体の位置と制御対象の作用端の位置との差から容易に求められるが、制御系が多自由度となるように複数の駆動系から構成されている場合には、各駆動軸に係るΔθSを計算する必要がある。
【０１３４】
そのためには、図２に示す移動物体検出手段３により、移動物体の位置を常に監視する必要がある。つまり、移動物体の位置を常時検出することによって、制御対象の作用端をその現在位置からどれくらい移動させれば、作用端の位置を移動物体の現在位置に一致させられるかを計算することができる。
【０１３５】
移動物体検出手段３としては、例えば、撮影手段によって得られる移動物体の画像情報からその現在位置を割り出すような構成を用いることができるが、移動物体がコンベア等の搬送手段によって運ばれるような場合には、搬送手段に設けられた位置検出器（ロータリーエンコーダ等）から移動物体の現在位置を知ることができる。いずれにしても、検出時間ができるだけ短い検出方法を採用することが、トラッキング動作の遅れを低減し、精度、安定性を保証する上で望ましい。
【０１３６】
移動物体の位置情報が得られると、これと制御対象の作用端の位置情報に基づいてΔθS等を計算することができることになるが、その計算の具体例について説明するために、制御対象を２軸のロボット系に特定して、ロボットを平面で見たときの構成を略線的に表現したものが図１４である。
【０１３７】
図中、ロボットＲＢは第１アームａ１と第２アームａ２とを有し、第１アームａ１の回動中心Ｏ1を原点とする２次元の直交位置座標系（Ｘ，Ｙ）が設定されており、点Ｅ（Ｘ，Ｙ）がロボットの手先位置を示している。尚、点Ｅの位置ベクトルをＲとすると、この場合Ｒ＝（Ｘ，Ｙ）であるが、一般には、Ｒ＝（Ｘ，Ｙ，Ｚ）である。
【０１３８】
また、点Ｏ2は第２アームａ２の回動中心であり、線分Ｏ1Ｏ2の長さ（これを「Ｌ1」とする。）が第１アームａ１の長さを示し、線分Ｏ2Ｅの長さ（これを「Ｌ2」とする。）が第２アームａ２の長さを示している。
【０１３９】
そして、「Θi（ｉ＝１、２）」は、各駆動軸に係る関節角を表しており、角度「Θ1」は線分Ｏ1Ｏ2がＸ軸に対してなす角度、角度「Θ2」は線分Ｏ2Ｅが線分Ｏ1Ｏ2の延長線に対してなす角度をそれぞれ示している。
【０１４０】
点Ｍ（ｘ，ｙ）は移動物体の位置を示しており（点Ｍの位置ベクトルをｒとすると、この場合ｒ＝（ｘ，ｙ）であるが、一般には、ｒ＝（ｘ，ｙ，ｚ）である。）、ｔｇは移動物体の軌跡を示している。
【０１４１】
今、図１４に２点鎖線で示すように、仮想ロボットＶＲ、つまり上記第１アームａ１及び第２アームａ２と同等の仮想アームａ１′、ａ２′からなるロボットを想定し、その手先位置が点Ｍにあるとすると、仮想アームに係る関節角θｉ（ｉ＝１、２）を求めることができる。
【０１４２】
つまり、角度「θ1」は仮想第１アームａ１′がＸ軸に対してなす角度、角度「θ2」は仮想第２アームａ２′が仮想第１アームａ１′の延長線に対してなす角度を示しており、これらの角度を手先位置の座標値（ｘ，ｙ）から計算する問題は逆運動学問題として知られている。
【０１４３】
ロボットＲＢについても同様にその手先位置の座標値（Ｘ，Ｙ）から関節角Θiを求めることができるので、関節角の差を「Δθi＝Θi−θi」（ｉ＝１、２）とすると、これらは下式のようになる。
【０１４４】
【数１９】
0003648812.tif 000022
【０１４５】
尚、上式において「ａｔａｎ（）」は正接関数の逆関数、「ａｃｏｓ（）」は余弦関数の逆関数をそれぞれ示している。
【０１４６】
上述した諸量等の定義を下表にまとめて示す。
【０１４７】
【表３】
0003648812.tif 000023
【０１４８】
よって、トラッキング動作の開始時点における移動物体や制御対象の作用端の位置情報を上記［数１９］式に代入すれば、各駆動軸についてのΔθS値を計算することができるので、各駆動軸について予め規定されているΔθS−ＴP特性からそれぞれのＴP値を各別に決定することができる。
【０１４９】
Ｔf値についても定点間移動に係る制御の場合と同じアルゴリズムに従ってＴf値を決定することができ、この処理を担当するのが図２に示すＴf値決定手段７である。即ち、ランプ信号に係る速度が制御対象に係る最大速度未満か否かに応じて、Ｔf＝ＴPと同じ値とするか、または、Ｔf＝ΔθS／ΩMAXとするかを決定する。
【０１５０】
例えば、図９において決定されるＴf値に基づいて生成されるランプ信号がランプ変調信号θ〜rmpのランプ信号成分θrmpに係る折れ時間であると考えれば良く、あるいは、図９のランプ信号が静止物体へのトラッキング動作に係るランプ変調信号であると考えれば良い。
【０１５１】
ランプ変調信号θ〜rmpは、ランプ信号θrmpに移動物体に係るθ〜（ｔ）を重畳することで得られる信号であり、該信号を生成するのが図２に示す変調信号生成手段８である。尚、θT（ｔ）は移動物体の位置情報に基づいてその位置に制御対象の作用端を移動させるのに必要な制御対象の構成軸に係る移動量から計算することができる。例えば、図１４に示す例では、その作用端の位置が移動物体の位置に一致する仮想ロボットの各関節角がθT（ｔ）に相当し、制御対象の構造から計算式を導出することができる（その具体的な計算法については本発明の要旨に直接関係ないので省略する）。
【０１５２】
上記したようにＴP値が求まると、これをその基準値ＴPMINで割ることでα値を決定することができ、この処理を図２のα値決定手段９が受け持っている。
【０１５３】
ところで、α値の決定については、これをトラッキング動作の開始時点で決定してそれ以後のα値を固定する方法（以下、「α値固定法」という。）と、移動物体と制御対象の作用端との間の角度差Δθiに応じてα値を時々刻々と変化させていく方法（以下、「α値可変法」という。）とを挙げることができ、以下これらについて説明する。
【０１５４】
ａ）α値固定法
トラッキング動作の開始時点における移動物体の位置座標を（ｘS、ｙS）とし、制御対象の作用端の位置座標を（ＸS、ＹS）とし、この時の角度差ΔθiをΔθiS（ｉ＝１、２）とすると、これらは［数１９］式から下式のように求まる。
【０１５５】
【数２０】
0003648812.tif 000024
【０１５６】
よって、上述したようにΔθiSからＴPiの値を求めることができる。即ち、ΔθiとＴPiとが下式［数２１］によって予め規定されている場合には、これにΔθi＝ΔθiSを代入することによってＴPiS（ｉ＝１、２）を下式［数２２］のように求めることができる。
【０１５７】
【数２１】
0003648812.tif 000025
【０１５８】
【数２２】
0003648812.tif 000026
【０１５９】
よって、ＴPiS（ｉ＝１、２）から一のＴP値（これを「ＴPS」とする。）を規定するには、例えば、下式のようにＴPMINを含めたものから最大のものを選び出せば良い。
【０１６０】
【数２３】
0003648812.tif 000027
【０１６１】
これによって、ＴPS値に対応するα値（これを「αS」とする。）が下式のように決定される。
【０１６２】
【数２４】
0003648812.tif 000028
【０１６３】
仮想サーボ系を用いる場合には、トラッキング動作の開始時点でαS値を決定して、基準系に対してα変換することにより、[Ｋi＝ＫiMIN／αS2，ＫP＝αS・ＫPMIN，Ｋ＝αS・ＫMIN，ＴPS＝αS・ＴPMIN]によりその特性が特定される仮想サーボ系を生成して、これに位置指令としてランプ信号を入力した結果得られる出力から加減速曲線が求められる。
【０１６４】
トラッキング動作開始後の接近動作においては、ランプ変調信号を仮想サーボ系の位置指令に関するノードに与えていけば、制御対象の作用端が移動物体に次第に近づいていき、遂にはトラッキングインに至る。
【０１６５】
図１５はその状況の一例を示すものであり、横軸に時間ｔをとり縦軸にθとαを併せて示すものである（尚、説明の便宜上制御対象が１軸で構成されているものとする。）。
【０１６６】
図示するようにトラッキング動作の開始時において、θTとθ〜rmpとの差はΔθSであるが、時間の経過とともに縮まり、ｔ＝ＴfでθTとθ〜rmpとが一致する。そして、この時点以後はθ〜rmpがθTに等しくなり、Ｔtr＝Ｔf＋ＴPSの時点でトラッキングインに至る。この間α＝αSで一定である。
【０１６７】
尚、ｔ＝Ｔfの時点以後においてθ〜rmp＝θTとなるように切り換えが行われるため、移動物体から制御対象の作用端を見た場合には、作用端の減速制御により両者の相対速度の差が次第に縮まって作用端が移動物体に忍び寄るように接近して来ることになる。
【０１６８】
また、制御対象が複数の駆動軸から構成される場合には、一般には駆動軸毎にαSの値が求められ、それぞれの駆動軸について各別のαS値を用いることができるが、制御の円滑化やアルゴリズムの単純化等を考慮すると、なるべく共通のαS値を用いる方が好ましい。
【０１６９】
ｂ）α値可変法
この場合には接近動作中の移動物体の位置座標や、制御対象の作用端の位置座標は時々刻々を変化し、角度差Δθi（ｉ＝１、２）は上記した［数１９］式から求められる。
【０１７０】
よって、上述したようにΔθiからＴPiの時々の値を求めることができる。即ち、ΔθiとＴPiとが上式［数２１］によって予め規定されている場合には、ＴPi（ｔ）（ｉ＝１、２）を下式のように求めることができる。
【０１７１】
【数２５】
0003648812.tif 000029
【０１７２】
よって、ＴPi（ｔ）（ｉ＝１、２）から一のＴP（ｔ）の値を規定するには、例えば、下式のようにＴPMINを含めたものから最大のものを選び出せば良い。
【０１７３】
【数２６】
0003648812.tif 000030
【０１７４】
α（ｔ）はＴP（ｔ）をその基準値ＴPMINで割ることによって得られることになるが、これらの量が時間ｔに依存することから分かるように、α値は時間とともに更新される。即ち、時間軸についてサンプリング期間ＴSで標本化の処理を行った場合に、あるサンプリング時点でのα値を「αNEW」とし、一つ前のサンプリング時点でのα値を「αOLD」とすると、α値を下式のように更新すれば良い。
【０１７５】
【数２７】
0003648812.tif 000031
【０１７６】
尚、上式において「ｍｉｎ（Ｘ１、Ｘ２）」は、Ｘ１、Ｘ２のうち小さい方を選び出すための関数である。
【０１７７】
仮想サーボ系を用いる場合には、接近動作中α（ｔ）の値を決定してこれに応じて基準系に対してα変換することにより、[Ｋi＝ＫiMIN／αNEW2，ＫP＝αNEW・ＫPMIN，Ｋ＝αNEW・ＫMIN，ＴP＝αNEW・ＴPMIN]によりその特性が特定される仮想サーボ系をその都度生成して、これらに位置指令θREFとしてランプ信号を入力した結果得られる出力から加減速曲線が求められる。
【０１７８】
尚、［数２７］式において、ｍａｘ（ＴP／ＴPMIN、αMIN）とαOLDとを比較することによってαがαOLDを越えないようにしている理由は、何等かの外乱によって「ｍａｘ（ＴP／ＴPMIN、αMIN）＞αOLD」となった時、つまり、移動物体と制御対象の作用端との間の角度差Δθiがより大きくなってしまった場合には、αが時間の経過につれて大きくなってしまうため、これによって仮想サーボ系の応答が遅くなり、さらに角度差Δθiが大きくなってしまうという悪循環を防止するためである。
【０１７９】
図１６は横軸にΔθをとり、縦軸にＴP及びαをとってαの変化の一例について示すものであり（Δθ−ＴP特性については図８と同じ。）、時間の経過につれてα値は矢印Ａに示すようにその最小値αMIN（＝１）に近づいていく。
【０１８０】
つまり、トラッキング動作開始後の接近動作においては、α値が次第に小さくなって行き、遂には１となって仮想サーボ系がその基準系に一致することになる。
【０１８１】
尚、トラッキングインの状況は移動物体と制御対象の作用端との運動状態によって様々であり、例えば、作用端が移動物体を後ろから追いかけていく場合や、作用端が移動物体の進行方向から近づいていく場合、作用端が移動物体の進行方向に対してある角度をもって近づいていく場合等が挙げられる。また、トラッキングイン時間ＴtrについてはＴPやＴf値が次第に小さくなっていくことからα値固定法に比べて短くなることが分かる。
【０１８２】
以上のようにα値固定法は、その制御が簡単であるという利点を有する反面、α値可変法のようなトラッキングイン時間Ｔtrの短縮化を期待することができず、他方、α値可変法は、トラッキングイン時間Ｔtrの短縮化や外乱への対策が可能であるという利点を有する反面、制御がやや複雑化することになるため、これらの点を考慮していずれの方法を採用するかを判断する必要がある。
【０１８３】
トラッキングインにおける処理は、α値固定法及びα値可変法のいづれについても同じであり、制御対象の作用端による移動物体の捕捉や制御対象の作用端の移動物体への接近を図２の捕捉／接近判定手段１０が判定する。
【０１８４】
即ち、捕捉／接近判定手段１０は移動物体に係る位置、速度、加速度の情報と、制御対象の作用端に係る位置、速度、加速度の情報とに基づいて、上記精度ベクトルεについての計算を行った上で、精度ベクトルの各成分がそれらの閾値内に収まっているか否かを判断する。その際、移動物体に係る位置等の情報は移動物体検出手段３から得られ、また、制御対象の作用端に係る位置等の情報は、制御対象が制御指令値通りに運動している場合には制御指令値から明らかであり、又は制御対象の作用端の運動状態を検出する作用端検出手段が制御対象内若しくは制御対象とは別個に設けられている場合にはこれらによって制御対象の実際の運動を監視することで位置等の情報を得ることできる。
【０１８５】
尚、制御対象が複数の駆動軸から構成される場合には、上記のような精度ベクトルεを用いたトラッキングインの判定を行う計算量が増えることが予想される。このような場合には、トラッキングイン時間ＴtrがＴf＋ＴPであるので、トラッキング動作の開始時点を起点とした時間を管理することによってトラッキングインに達したことを判定するのが簡単である。但し、このようにトラッキングイン時間をトラッキング動作の開始時において事前に知ることができるのは、速度や加速度に飽和がないとき、つまり、図１２で説明したような速度や加速度についてそれらの最大値による制限が働かない場合であることに注意する（速度や加速度が制限を受ける場合にはＴtr＝Ｔf′（＞Ｔf）＋ＴPとなり、Ｔf′はランプ変調信号の受ける制限の如何に応じて変化する。）。
【０１８６】
捕捉／接近判定手段１０による判定出力は、図２に示すように、加減速パターン生成手段１１と制御対象との間に設けられた切換手段１２の制御信号として用いられる。即ち、切換手段１２は、捕捉／近接判定手段８の信号を受けて、制御対象１３の作用端が移動物体を捕捉し又は作用端が移動物体に近接したことが判定されるまでの間は、変調信号生成手段８の出力するランプ変調信号θ〜rmpが加減速パターン生成手段１１を介して制御対象に送出されるようにし、また、制御対象１３の作用端が移動物体を捕捉し又は作用端が移動物体に近接したことが判定された場合には、制御指令値計算手段５から変調信号生成手段８を介して送られてくるθTをそのまま制御対象１３に送出するように切り換える。つまり、トラッキングイン後に移動物体の位置等の情報を制御対象１３への制御目標値として直接送出することによって追従動作の精度を向上させることができ、制御の遅れの影響等を排除することができる。
【０１８７】
尚、精度ベクトルεについての判定の閾値は切換手段１２の切り換えが制御対象の運動に影響を及ぼさないように注意して適当な値に設定する必要がある。また、トラッキングインのタイミングを「Ｔtr＝Ｔf＋ＴP」に基づく計時により判定するか否かは制御対象を構成する駆動軸の数の如何に応じて判断すれば良い。追従動作中は、捕捉／接近判定手段１０による判定処理が常時行われ、制御対象の作用端が所定の精度でもって移動物体に追従しているか否かがチェックされる。
【０１８８】
尚、図２ではθTが変調信号生成手段８から切換手段１２に送出されるようになっているが、θTが元来制御指令値計算手段５において計算されるものであるから該制御指令値計算手段５からθTを切換手段１２に送出するようにしても良い。  しかして、以上の説明から本発明に係るサーボ制御装置１における構成要素（図２参照。）間の関係が明らかとなる。
【０１８９】
即ち、指令手段２によってトラッキング動作の開始指令が、制御指令値計算手段５に送られると、該制御指令値計算手段５は移動物体の位置情報及び制御対象の作用端の位置情報に基づいて、制御対象１３の作用端の位置を移動物体の位置に一致させるために制御対象の作用端をどれくらい動かせばよいかについての制御値（Δθやθi等）や移動物体の位置に対応するθTを計算する。尚、移動物体の位置ベクトルｒ＝（ｘ、ｙ、ｚ）は移動物体検出手段３から得られ、また、制御対象の作用端の位置ベクトルＲ＝（Ｘ，Ｙ，Ｚ）は制御上既知であるか又は付設の作用端検出手段から得られる。
【０１９０】
そして、制御指令値計算手段５の出力はＴP値決定手段６及びＴf値決定手段７に送出される。
【０１９１】
ＴP値決定手段６は、予め規定されているΔθ−ＴP特性に基づいてＴP値を求めてこれをα値決定手段９に送出する。そして、α値決定手段９はＴP値をその基準値で割ることによってα値を決定してこれを加減速パターン生成手段１１に送出する。
【０１９２】
また、Ｔf値決定手段７は、ランプ信号に係る速度が制御対象に係る最大速度未満か否かに応じて、Ｔf＝ＴP又はＴf＝Δθ／ΩMAXによりＴf値を決定して、これを変調信号生成手段８に送出する。
【０１９３】
変調信号生成手段８は、Ｔf値決定手段７からのＴfや制御指令値計算手段５からのΔθ、θTに基づいて上記したランプ変調信号θ〜rmpを生成してこれを加減速パターン生成手段１１や切換手段１２に送出する。つまり、トラッキング動作の開始時点を起点として０≦ｔ≦Ｔfの期間中は信号θ〜rmp＝θrmp＋θTが加減速パターン生成手段１１に送出され、Ｔf≦ｔ≦Ｔtrの期間中は信号θ〜rmp＝θTが加減速パターン生成手段１１に送出され、ｔ＞Ｔtrでは信号θTが切換手段１２に送出される（Ｔf≦ｔにおけるランプ変調信号はθ〜rmp＝θTであるのでこれが切換手段１２においてθTが選択される。）。
【０１９４】
加減速パターン生成手段１１は、変調信号生成手段８やα値決定手段９からの情報に基づいて加減速パターンを生成して、該加減速パターンに沿う制御信号を切換手段１２を介して制御対象に送出する。
【０１９５】
捕捉／接近判定手段１０は、移動物体検出手段３からの情報（ｒ、ｒ(1)、ｒ(2)）や制御対象の作用端の情報（Ｒ、Ｒ(1)、Ｒ(2)）に基づいてトラッキングインについての判定を行い、判定結果に応じて切換手段１２を切り換えることによって、ランプ変調信号θ〜rmpを直接制御対象に送出するか又は加減速パターン生成手段１１を介して制御対象１３に送出するかの選択を行う。また、捕捉／接近判定手段１０は追従動作中において精度ベクトルεが所定の範囲内に入っている状態で制御対象の作用端の移動物体への追従が行われているか否かを監視し、トラッキングアウトの判定を下す。
【０１９６】
尚、サーボ制御装置１において捕捉／接近判定手段１０や切換手段１２が構成上必須のものでないことに注意する。何故なら、上述したようにトラッキングイン時間が「Ｔtr＝Ｔf＋ＴP」に基づいて事前に分かる場合には、精度ベクトルεによるトラッキングインの判定は必要でなく、また、切換手段１２は追従動作の精度の向上や制御の遅れの防止を考慮して場合に必要されるもので、これらについて何等心配がない場合には、トラッキングインに関係なく加減速パターン生成手段１１による制御信号を常に制御対象１３に送出することができるからである。
【０１９７】
本発明に係るサーボ制御方法をまとめると、α値固定法の場合には図１７に示すようになり、α値可変法の場合には図１８に示すようになる。
【０１９８】
α値固定法の場合には、図１７のステップＳ１においてトラッキング動作の開始時点での移動物体や制御対象の作用端の位置から［数２０］式に示したように制御指令値ΔθisやθTを計算する。
【０１９９】
ステップＳ２では、トラッキングインの判定を行い、未だトラッキングインに至っていない場合にはステップＳ３に進み、トラッキングインの場合にはステップＳ１２に進む。
【０２００】
そして、次ステップＳ３はこれに基づいてＴPSやαS値を決定する（［数２３］式、［数２４］式参照。）。
【０２０１】
次ステップＳ４においてＴf値を決定した後、ステップＳ５においてランプ変調信号θ〜rmpを生成する。尚、このステップＳ５では上述したような速度や加速度についての制限が課せられる。
【０２０２】
そして、ステップＳ６においてトラッキング動作の開始時点を起点とする時間ｔがＴfに達したか否かが問われ、ｔ≦ＴfならばステップＳ７に進み、そうでなければステップＳ１０に進む。
【０２０３】
ステップＳ７においてランプ変調信号θ〜rmpとしてθrmp＋θ〜を加減速パターン生成手段１１に送出し、次ステップＳ８において上述のように関数計算による生成法や仮想サーボ系を用いる方法によって加減速パターンを生成する。
【０２０４】
そして、ステップＳ９において加減速パターンに従う制御信号を制御対象に送出した後、ステップＳ５に戻る。
【０２０５】
尚、接近動作中はステップＳ５乃至ステップＳ９が繰り返されることになるが、この間、移動物体の位置変化に応じて制御対象の作用端の目標位置が変化するので、制御目標（つまり、θT）に係る再計算が行われる。
【０２０６】
ステップＳ６においてｔ＞Ｔfが判断されるとステップＳ１０に進み、トラッキングインの判定が行われる。そして、未だトラッキングインに至っていない場合にはステップＳ１１に進み、ランプ変調信号θ〜rmpとしてθTを加減速パターン生成手段１１に送出してステップＳ８に進む。
【０２０７】
ステップＳ２又はステップＳ１０においてトラッキングインが判定されるとステップＳ１２に進み、θTを制御対象に直接送出する。
【０２０８】
そして、次ステップＳ１３においてトラッキングアウトに関する判定がなされ、追従動作の継続中であればステップＳ１２に戻り、トラッキングアウト時にはステップＳ１に戻る。
【０２０９】
α値可変法の場合には、図１８に示すように、ステップＳＳ１において移動物体や制御対象の作用端の位置から［数１９］式に示したように制御指令値ΔθやθT等を計算する。
【０２１０】
そして、ステップＳＳ２においてトラッキングインの判定を行い、未だトラッキングインに至っていない場合にはステップＳＳ３に進み、トラッキングインの場合にはステップＳＳ１３に進む。
【０２１１】
ステップＳＳ３では［数２５］式及び［数２６］式に示したようにＴP（ｔ）の値を決定し、続くステップＳＳ４においてＴf値を決定する。
【０２１２】
そして、ステップＳＳ５においてα（ｔ）の値を決定する。尚、ステップＳＳ４とＳＳ５とが逆になっても構わない。
【０２１３】
ステップＳＳ６においてランプ変調信号θ〜rmpを生成する。尚、このステップＳＳ６において上述したような速度や加速度についての制限が課せられる。
【０２１４】
そして、ステップＳＳ７においてトラッキング動作の開始時点を起点とする時間ｔがＴfに達したか否かが問われ、ｔ≦ＴfならばステップＳＳ８に進み、そうでなければステップＳＳ１１に進む。
【０２１５】
ステップＳＳ８においてランプ変調信号θ〜rmpとしてθrmp＋θ〜を加減速パターン生成手段１１に送出し、次ステップＳＳ９において上述のように関数計算による生成法や仮想サーボ系を用いる方法によって加減速パターンを生成する。
【０２１６】
そして、ステップＳＳ１０において加減速パターンに従う制御信号を制御対象に送出した後、ステップＳＳ１に戻る。
【０２１７】
尚、接近動作中はステップＳＳ１乃至ステップＳＳ１０が繰り返され、この間、［数２７］式に示したようにα値が更新されていく。
【０２１８】
ステップＳＳ７においてｔ＞Ｔfが判断されるとステップＳＳ１１に進み、トラッキングインの判定が行われる。そして、未だトラッキングインに至っていない場合には、ステップＳＳ１２に進み、ランプ変調信号θ〜rmpとしてθTを加減速パターン生成手段１１に送出してステップＳＳ９に進む。
【０２１９】
ステップＳＳ２又はステップＳＳ１１においてトラッキングインが判定されるとステップＳＳ１３に進み、θTを制御対象に直接送出する。
【０２２０】
そして、次ステップＳＳ１４においてトラッキングアウトに関する判定がなされ、追従動作の継続中であればステップＳＳ１３に戻り、トラッキングアウト時にはステップＳＳ１に戻る。
【０２２１】
尚、制御対象や移動物体の運動状態に応じて、指令手段２からトラッキング制御手段４に送出される指令によってα値固定法やα値可変法とを適宜に使い分けることができるようにすると処理の総括化が可能である。
【０２２２】
しかして、本発明によれば、定点間移動に係る制御とトラッキング動作に係る制御とが統一化され、両者は全く同じアルゴリズムに従って加減速パターンが生成される。
【０２２３】
即ち、定点間移動の場合には、移動物体に係るθTが時間に依らず一定（＝ΔθS）、つまりθ〜＝０であるので、図２のサーボ制御装置１における変調信号生成手段８は単にランプ信号θrmpを加減速パターン生成手段１１に送出することになる。尚、この場合、切換手段１２は常に加減速パターン生成手段１１の出力を選択した状態となる。よって、制御手順は図１７や図１８におけるトラッキングイン等の判定ステップ（Ｓ２、Ｓ１０、Ｓ１３やＳＳ２、ＳＳ１１、ＳＳ１４）やトラッキングイン後にθTを制御対象に直接送出するステップ（Ｓ１２やＳＳ１３）が不要となり、ステップＳ１１やＳＳ１２におけるθ〜rmpがＴf≦ｔ≦Ｔf＋ＴPの期間中一定値ΔθSとなるだけである。
【０２２４】
以上の説明から本発明の全容が明らかとされたが、トラッキング動作の制御に関連する幾つかの事項について説明する。
【０２２５】
その１は、トラッキングイン時間Ｔtrの制御であり、Ｔtrを自由に指定することができれば、目標物の捕捉や接近に要する時間について管理することができるようになる。
【０２２６】
図１９は、トラッキングイン時間をｐ倍（図ではｐ＝２）に指定する場合の状況を示すｔ−θ図である。
【０２２７】
今、トラッキングイン時間の最小値（これを「Ｔtr100」とする。）を１００％として、トラッキングイン時間をｐ倍に伸ばすときの伸縮率（これを「Ｐ」と記す。）を「Ｐ（％）＝１００／ｐ」で定義すると、図１９において伸縮率１００％のランプ変調信号（これを「θ〜rmp100」とする。）がｔ＝Ｔfで移動物体に係るθTに追い付きＴtr100＝Ｔf＋ＴPにおいてトラッキングインに達し、また、伸縮率Ｐ％のランプ変調信号（これを「θ〜rmpP」とする。）がｔ＝Ｔf′で移動物体に係るθTに追い付きトラッキングイン時間（これを「ＴtrP」とする。）がＴtrP＝Ｔf′＋ＴPとなる。
【０２２８】
つまりＴtrP＝（１００／Ｐ）・Ｔtr100から下式に示すようにＴf′が決定される。
【０２２９】
【数２８】
0003648812.tif 000032
【０２３０】
よって、伸縮率Ｐの場合のランプ変調信号は、前記［数１４］式におけるＴfを上記［数２８］式のＴf′で置換することによって下式に示すようになり、これによってトラッキングイン時間がｐ倍（つまり、１００／Ｐ倍）となる。このような伸縮率Ｐに係る計算は図２において指令手段２からＴf値決定手段７に送出される信号Ｓovによって指示される。
【０２３１】
【数２９】
0003648812.tif 000033
【０２３２】
尚、定点間移動についてもこのような制御が同じ手順によって可能であることは明白である。何故なら、［数２９］式においてθ〜＝０、θT＝ΔθSとすれば良いからである。
【０２３３】
但し、［数２８］式によるトラッキングイン時間ＴtrPの制御については、図２０に示すようなトラッキング動作が行われる場合が生じうるので、そのための対策を講じることが望ましい。
【０２３４】
図２０は２つの駆動軸を有するロボット系に制御対象を特定し、円軌道を描いて一定の速さで運動する移動物体に対してロボットＲＢの作用端に係るトラッキング動作を行う状況を示すものであり、空間座標軸として横軸にＸ軸、縦軸にＹ軸を設定するとともに原点ＯをロボットＲＢの基軸部に設定している。尚、ロボットＲＢはその第１アームａ１が基軸部に対して回動可能に支持され、第２アームａ２が第１アームａ１に対して回動可能に支持されており、第２アームａ２の端部に作用端が設けられており、図示するロボットＲＢの状態はトラッキング動作の開始時点における状態を示している（ロボットＲＢの作用範囲ＡＲは図の半円内に示す領域である）。
【０２３５】
図中、Ｙ軸上に中心Ｃを有する円軌道Ｌtは移動物体の軌跡を示しており、該移動物体への接近動作中にロボットＲＢの作用端が軌跡ＬRに沿って移動し、円軌Ｌt上の点Ｑで作用端が移動物体を捉える様子が示されている。軌跡ＬRは上記したような伸縮率Ｐのトラッキング動作に対応する一例を示すものであり、トラッキングイン時間が伸ばされたために、迂回路、例えば、ループｌを描く事によって時間調整を行っているように見える。このようなループｌの存在は、経路途中に障害物があるような場合には許されないことになるため、作用端がなるべく迂回路を採らないように移動物体に接近することが好ましい。
【０２３６】
そこで、伸縮率Ｐのトラッキング動作のトラッキングイン時間が上記［数２８］式をもとにして、下式［数３０］により計算される遅延時間（これを「Ｔd」とする。）だけ待ってからｔ＝Ｔd後に折れ時間Ｔfのランプ信号成分を有するランプ変調信号を用いてトラッキング動作を開始すれば、トラッキングイン時間がＴd＋（Ｔf＋ＴP）＝Ｔf′＋ＴPとなり、伸縮率Ｐのトラッキング動作に係るトラッキングイン時間と同じになる。
【０２３７】
【数３０】
0003648812.tif 000034
【０２３８】
尚、上記の説明ではトラッキングイン時間の最小値Ｔtr100を伸縮率の基準として選んでいるため、伸縮率Ｐが１００％を越えることはないので、伸縮率Ｐは伸張率と呼べるものであるが、基準値の選び方は全く任意であること（例えば、Ｔtr50を基準とする等）から「伸縮率」という用語の妥当性が容認される。
【０２３９】
ところで、トラッキング動作の開始指令が発せられる前には、各種の判断が必要になり、その中でも現時点からトラッキング動作を開始したとして、果たして制御対象の作用端の移動物体へのトラッキングインが可能かどうかの判断が重要である。このような判断を行わないと、例えば、トラッキング動作を開始してからトラッキングインの前に移動物体が制御対象の守備領域から外に出てしまう場合にせっかくの動作が無駄な努力になってしまい、作業効率等の低下させる原因となる。
【０２４０】
図２１は、図２０のロボットＲＢについてその作業領域ＡＲ内を通過するコンベア（直線Ｇで示す。）上に移動物体に相当するワークが載置されて搬送されて来る状況を示すものであり、点ＣSが直線Ｇと作業領域ＡＲとの交点のうち作業領域ＡＲへの進入点を示し、また、点ＣEが直線Ｇと作業領域ＡＲとの交点のうち作業領域ＡＲからの脱出点を示しており、直線Ｇ上の点ＣTがワークの位置を示している。また、線分ＣSＣEの長さが「ＬC」、線分ＣSＣTの長さが「ｌC」であり、コンベアの搬送速度を「Ｖ」とすると、ワークが作業領域内に滞在する時間（これを「ＴCH」とする。）は「ＬC／Ｖ」である。
【０２４１】
そして、ロボットＲＢの作用端がワークに対して働きかけるのに要する時間を「ＴZR」とし、制御対象に係る制御の遅れ時間を「Ｔsvd」とすると、トラッキングインについての可又は不可の判定を、次式のように下すことができる。尚、諸量の定義をまとめると下表４のようになる。
【０２４２】
【数３１】
0003648812.tif 000035
【０２４３】
【表４】
0003648812.tif 000036
【０２４４】
図２２はランプ変調信号θ〜rmpとθTとを併せて示すｔ−θ図であり、点Ａは作用端が移動物体に追い付く時点での位置、点Ｂはトラッキングインの時点での位置をそれぞれ示している。「ＴCH*」は判定の閾値（［数３１］式参照。）に等しい。）を示しており、ＴCH＝ＴCH1（＜ＴCH*）の場合には、現時点（ｔ＝０）からのトラッキング動作が間に合い、ＴCH＝ＴCH1（≧ＴCH*）の場合にはトラッキング動作が間に合わないことになるので、後者の場合にはその旨を隣接するロボットに伝達して当該ロボットに処理を委ねる等、適宜の処置をとれば良い。
【０２４５】
また、場合によっては、トラッキングインにおける位置を指定したい場合があり、このようなときには、図２１において線分ＣSＣTの長さｌCをコンベアの搬送速度Ｖで割った時間を用いて、下式のようにＴfを決めれば、点ＣTにおいて丁度トラッキングインが達成される。
【０２４６】
【数３２】
0003648812.tif 000037
【０２４７】
【実施例】
以下に、本発明をロボット制御装置に適用した実施の一例を図２３乃至図２９に従って説明する。
【０２４８】
図２３はロボット制御装置１４のハードウェア構成の要部を概略的に示すものである。
【０２４９】
ロボット制御装置１４は、ＣＰＵ（中央演算装置）１５を核とし、そのバス１６に直接又は間接的に接続される記憶部１７を有している。
【０２５０】
記憶部１７には、例えば、ロボットの動作手順の記述に係るロボットプログラムや、ロボットシステムの記述（例えば、ロボットの各軸の動作を決定する座標変換等の記述）に係るシステムプログラムが記憶されるようになっている。
【０２５１】
サーボユニット１８、１８、・・・は、ロボット１９を構成する駆動軸毎に独立しており、ＣＰＵ１５から発せられた命令に従ってロボット１９の軸駆動を行なうアクチュエータ（モータやシリンダー等）を制御するために設けられている。
【０２５２】
ロボット１９の動作指示に係るティーチングペンダント２０は、Ｔ．Ｐ．（ティーチングペンダントの略）インターフェース２１を介してバス１６に接続されている。
【０２５３】
図２４は、制御対象となるロボット１９の構成例として水平多関節型ロボットを示すものである。
【０２５４】
即ち、ロボット１９は、基軸部２２と、第１アーム２３、第２アーム２４とを備え、第１アーム２３の一端部が回動し得る状態で基軸部２２に取り付けらている。そして、第１アーム２３の他端部には第２アーム２４の一端部が回動し得る状態で取り付けられており、第２アーム２４の他端部にはツール搭載軸２５が設けられている。
【０２５５】
ツール搭載軸２５の端部には効果器２６（ハンド等）が取り付けられ、これが制御対象の作用端である。
【０２５６】
以上のように、ロボット１９は、第１アーム２３の回動軸（θ1軸）、第２アーム２４の回動軸（θ2軸）、ツール搭載軸２５（Ｒｚ軸）のそれぞれに関するサーボ系によって構成されている。
【０２５７】
よって、ロボット制御装置１４への適用にあたっては、図２に示すサーボ制御装置１の構成を、ロボット１９の駆動軸毎に構築する必要がある。即ち、図２のサーボ系が、ロボット１９の駆動軸の一に相当するものと考えれば良く、ロボット制御に係るサーボ制御手段は上記ＣＰＵ１５、記憶部１７、サーボユニット１８やティーチングペンダント２０等の機能的結合によって実現される。
【０２５８】
尚、このようにロボットが多軸構成となっている場合には、各駆動軸についてα値が存在するので、それらを各別の値として取り扱うか、各駆動軸に共通のα値を用いるかを、制御の目的を考慮した上で使い分けるのが望ましい。
【０２５９】
尚、上記のロボットの姿勢は、図２５に示すようにｔ−θ2図とｔ−θ1図によって記述されることは、これらの図において時々のロボットの姿勢を略線的に示す図（図の最下段に示す。）から明らかである。
【０２６０】
また、ロボット１９の第１アーム２３と第２アーム２４については、それぞれのアームについてＴP値やＴf値が独立に決定され、各アームについての値を比較して大きい方の値を採用すれば良いが（図１０、［数１２］式参照。）、ＴP値の決定には、例えば、第１アーム２３と第２アーム２４との間の力学的な干渉作用（つまり、一方のアームの運動が他方のアームの運動に対して影響を及ぼす力学的な作用であり、例えば、特開平４−３０６７１１号公報参照。）等を考慮した上で決定される各軸についてのＴP値から適切な値を選択することができる。
【０２６１】
ところで、上記した定点間移動とトラッキング動作との統一化は、ロボットの動作制御においてはＰＴＰ動作とトラッキング動作との統一化を意味し、両者について同じアルゴリズムを用いることはロボットの作業効率を向上させる上で効果的である。
【０２６２】
即ち、ロボットの動作に関して作業効率の向上を図るにはロボット自身の動きを高速化し、タクトタイム（作業時間）の短縮化を図ることを真先に挙げることができるが、これに限らず、ロボットの作用端を常に運動させておき、ある動作から別の動作に速やかに移行するように制御を行うことが有効な手段となる。
【０２６３】
このように、ロボットの動きを止めることなくある動作から別の動作へと移行させることによってロボットの停止により時間のロスを低減するようにした制御（以下、「ノンストップ制御」といい、このような制御が可能なロボットを「ノンストップロボット」という。）を採用すると、加速時や減速時における消費エネルギーが低減され、駆動源の消費電流等が小さくなり、駆動源の小容量化や小型化、延いてはロボットの小型化を図ることができるという利点がある。
【０２６４】
ＰＴＰ動作とトラッキング動作との間のアルゴリズムの統一化はノンストップ制御にとって都合の良いものであり、例えば、ＰＴＰ動作中に他の対象物に対してトラッキング動作を行いたい場合や、その逆の場合に時間的に大きなロスを伴うことなく動作間の移行が可能となる。尚、このようにロボットがある動作から他の動作に移ることを「ロボット動作状態遷移」と呼び、ロボットのあらゆる動作についてロボット動作状態遷移を速やかに可能にする機能がノンストップロボットの性能に深く関係している。
【０２６５】
そこで、一例としてＰＴＰ動作からトラッキング動作への状態遷移について説明する。
【０２６６】
図２６はＰＴＰ動作に係るランプ信号の一例を示すｔ−θ図であり、図中のｔ＝ＴSTに示す時点でトラッキング動作を開始したいものとする。尚、図中に示す諸量のうち下付きの添え字「PTP」は、ＰＴＰ動作に係る量であることを意味している。各量の定義を表にまとめたものが下表である。
【０２６７】
【表５】
0003648812.tif 000038
【０２６８】
今、ＴSTのタイミングとΔθtrの大きさによって起こり得る状況を場合分けすると下記のように４通りになる。
【０２６９】
（ｉ）Δθtr≧０かつ０≦ＴST≦ＴfPTPの場合。
【０２７０】
作用端の移動先にトラッキング動作に係る移動物体が存在して、トラッキング動作の開始時点ではランプ信号の傾斜部上の位置に作用端がある状況である。
【０２７１】
（ｉｉ）Δθtr≧０かつＴfPTP＜ＴST≦ＴfPTP＋ＴPの場合。
【０２７２】
作用端の移動先にトラッキング動作に係る移動物体が存在して、トラッキング動作の開始時点ではランプ信号の平坦部（θ＝ΔθPTP）上の位置に作用端がある状況である。
【０２７３】
（ｉｉｉ）Δθtr＜０かつ０≦ＴST≦ＴfPTPの場合。
【０２７４】
作用端の移動先とは反対方向にトラッキング動作に係る移動物体が存在して、トラッキング動作の開始時点ではランプ信号の傾斜部上の位置に作用端がある状況である。
【０２７５】
（ｉｖ）Δθtr＜０かつＴfPTP＜ＴST≦ＴfPTP＋ＴPの場合。
【０２７６】
作用端の移動先とは反対方向にトラッキング動作に係る移動物体が存在して、トラッキング動作の開始時点ではランプ信号の平坦部（θ＝ΔθPTP）上の位置に作用端がある状況である。
【０２７７】
先ず、（ｉ）の場合には、移行時に下記に示すよう処理を行う。
【０２７８】
（１）α（時間軸伸縮用パラメータ）の値はＰＴＰ動作に係るα値のままとする。
【０２７９】
（２）ランプ信号の傾斜（＝ΔθPTP／ＴfPTP）はそのままとし、θ（ＴST）＋Δθtrに達した時点で折れ曲がるようなランプ信号を生成する。
【０２８０】
（３）（２）で得られたランプ信号に、θ〜（＝θT−Δθtr）をｔ＝ＴST以後に重畳することによってランプ変調信号を生成し、これと（１）のα値に基づいて加減速パターンを生成する。
【０２８１】
図２７はΔθtrについて２つの値ΔθtraとΔθtrbを例示して、各場合のランプ信号やＴf値を示すｔ−θ図であり、図に実線で示す線がＰＴＰ動作において予定されるランプ信号を示し、１点鎖線が折れ時間Ｔfa（つまり、θ≦Δθtra≦ΔθPTPの場合のＴf）のランプ信号、２点鎖線が折れ時間Ｔfb（つまり、θ＋Δθtrb≧ΔθPTPの場合のＴf）のランプ信号を示しており、これらを成分に含むランプ変調信号を用いてトラッキング動作への移行が果たされる。
【０２８２】
また、上記（ｉｉ）の場合には、移行時に下記に示すよう処理を行う。
【０２８３】
（１）α（時間軸伸縮用パラメータ）の値はＰＴＰ動作に係るα値のままとする。
【０２８４】
（２）ｔ＝ＴSTの時点で、ＰＴＰ動作に係るランプ信号と同じ傾斜（＝ΔθPTP／ＴfPTP）を有しかつΔθPTP＋Δθtrに達した時点で折れ曲がるようなランプ信号を生成する。
【０２８５】
（３）（２）で得られたランプ信号に、θ〜（＝θT−Δθtr）をｔ＝ＴST以後に重畳することによってランプ変調信号を生成し、これと（１）のα値に基づいて加減速パターンを生成する。
【０２８６】
図２８はこの様子を示すｔ−θ図であり、図に実線で示す線がＰＴＰ動作において予定されるランプ信号を示し、１点鎖線が折れ時間Ｔftrのランプ信号を示しており、これを成分に含むランプ変調信号を用いてトラッキング動作への移行が果たされる。
【０２８７】
尚、（ｉｉｉ）や（ｉｖ）の場合、つまりΔθtr＜０の場合にはｔ＝ＴSTの時点でＰＴＰ動作を直に止めるためにそのランプ信号を折り曲げてから単にトラッキング動作を開始すれば良い。例えば、図２９に示すようにＰＴＰ動作に係るランプ信号が実線で示すように予定される場合に、同図に１点鎖線で示すように、ｔ＝ＴSTで示す途中の時点でランプ信号を時間軸ｔに平行となるように折り曲げて時間ＴPの経過を待ってから、Δθtr、折れ時間Ｔftrにより決まるランプ信号にθ〜を加えてランプ変調信号を生成してトラッキング動作に係る制御を行えば良い。
【０２８８】
また、トラッキング動作の途中でＰＴＰ動作に移行した場合には、単にトラッキング動作を完全に止めた時点の作用端の位置と目標物の位置とに基づいてＰＴＰ動作に係る制御を行えば良いことはこれまでの説明から明らかである。
【０２８９】
しかして、本発明に係るサーボ制御装置及びサーボ制御方法をロボットに係るトラッキング動作制御に適用した場合には、ロボットの構造が如何なるものであっても、ロボットを構成する各駆動軸について統一されたアルゴリズムに従ってトラッキング動作の制御に係る加減速パターンを生成することができるので、トラッキング動作のためのアルゴリズムをロボットの構造に応じて個別に模索する必要がなくなる。そして、ＰＴＰ動作とトラッキング動作との統一化により、それぞれの動作に専用のアルゴリズムを必要とすることがなくなり、ＰＴＰ動作からトラッキング動作へ又はその逆のロボット動作状態遷移を速やかに行うことによってノンストップロボットを実現して作業の効率化を図ることができる。
【０２９０】
【発明の効果】
以上に記載したところから明らかなように、請求項１や請求項５に係る発明によれば、対象物の移動経路の予測が不能な場合でも対象物への接近及び追従が可能となり、しかも、該制御と、制御対象の作用端を静止物体又は定位置へと移動させる制御との統一化を図ることができる。
【０２９１】
そして、請求項２や請求項６に係る発明によれば、制御対象の運動性能を仮想的なサーボ系に反映させることによって性能を考慮した加減速曲線を生成することができるので、制御対象に対して無理な動作を強いることがなく、制御対象の作用端を移動物体にスムーズに接近させたり、移動物体をソフトに捕捉することができる。
【０２９２】
また、請求項３や請求項７に係る発明によれば、制御対象の作用端が移動物体を捕捉し又は作用端が移動物体対象物に近接したことを判定し、判定結果に応じて変調信号をそのまま制御対象に送出するか加減速パターン生成手段を介して制御対象に送出するかを規定することによって、追従動作に係る制御精度の向上を図り、制御に遅れが生じないようにすることができる。
【０２９３】
請求項８に係る発明によれば、ピーク速度到達時間及び時間軸伸縮用パラメータ値を決定した後、時間軸伸縮用パラメータ値を固定して加減速曲線を生成することによって、加減速曲線の生成の手続を簡単化することができる。
【０２９４】
請求項９に係る発明によれば、制御指令値計算手段による制御指令値の変化に応じて更新されるピーク速度到達時間及び時間軸伸縮用パラメータ値に基いて加減速曲線の時間軸方向の幅を制御することによって、制御対象の作用端による移動物体の捕捉又は制御対象の作用端の移動物体への接近の完了までに要する時間、つまり、トラッキングイン時間を短縮したり外乱への対応が可能である。
【０２９５】
請求項４や請求項１０に係る発明によれば、トラッキングイン時間の長さを伸縮させるための指令に応じて、ランプ信号に係る新たな折れ時間を求めてこれに基づく変調信号を生成することによって、トラッキングイン時間を自由に制御することができるので、制御の柔軟性が向上する。
【０２９６】
請求項１１や請求項１２に係る発明によれば、ロボットの構造がどのようなものであっても、統一化された加減速曲線の生成アルゴリズムに従ってロボットを構成する各サーボ系を制御することができ、ロボットの作用端を移動物体に接近させてこれを捕捉したり又はロボットの作用端を移動物体に追従させることができる。そして、ＰＴＰ動作とトラッキング動作との統一化によりロボットの作業効率を高めることができる。
【図面の簡単な説明】
【図１】制御対象の作用端の移動物体への接近について説明するものであり、（ａ）は位置差について、（ｂ）は速度差について、（ｃ）は加速度差についてそれぞれ説明するための図である。
【図２】本発明に係るサーボ制御装置の構成を示す図である。
【図３】関数計算による加減速曲線の生成について説明するためのグラフ図である。
【図４】仮想サーボ系について説明するためのものであり、（ａ）は現実のモータ系の一例を示すシグナルフロー図、（ｂ）は（ａ）のモータ系に対する仮想モータ系の構成例を示すシグナルフロー図である。
【図５】仮想モータ系にランプ信号を入力する場合の応答出力について説明するための図であり、（ａ）はランプ入力信号を示し、（ｂ）は出力θo及びθo(1)を示す。
【図６】仮想モータ系に係る基準系と、該基準系に対するα変換によって得られる仮想モータ系について説明するための図である。
【図７】定点間移動に係る加減速パターンの生成について説明するための図である。
【図８】Δθ−ＴP特性の一例を示すグラフ図である。
【図９】ＴP値及びＴf値の決定の仕方について説明するための図であり、（ａ）はｔ−θ図とＴP−Δθ図とを併記したグラフ図、（ｂ）は（ａ）に対応するｔ−Ω図である。
【図１０】制御対象が２軸構成の場合のＴP値及びＴf値の決定について説明するための図であり、ｔ−θ図とＴP−Δθ図とを併記して示すグラフ図である。
【図１１】ランプ信号の傾きと制御対象の最大速度との関係について説明するためのｔ−θ図である。
【図１２】ランプ信号についての速度や加速度の制限法について概念的に示す図である。
【図１３】ランプ変調信号等の定義量の関係を示すｔ−θ図である。
【図１４】制御対象を２軸構成のロボットに特定した場合に、ロボットや移動物体に関する諸量の関係を示す略線図である。
【図１５】α値固定法について説明するためのｔ−θ、α図である。
【図１６】α値可変法について説明するためのΔθ−ＴP、α図である。
【図１７】α値固定法を用いた場合の制御手順を示すフローチャート図である。
【図１８】α値可変法を用いた場合の制御手順を示すフローチャート図である。
【図１９】トラッキングイン時間の可変制御について説明するためのｔ−θ図である。
【図２０】円軌道を描いて運動する移動物体への接近動作の一例を示す概略図である。
【図２１】図２２とともにトラッキング動作の可・不可問題について説明するための図であり、本図はコンベアにより搬送される移動物体とロボットの作業領域との位置関係を示す概略図である。
【図２２】ランプ変調信号θ〜rmp及び移動物体に係るθTを示すｔ−θ図である。
【図２３】図２４及び図２５とともにロボット制御装置への適用について示すものであり、本図はロボット制御装置のハードウェア構成例を示す図である。
【図２４】水平多関節型ロボットの一例を示す側面図である。
【図２５】ｔ−θ2図、ｔ−θ1図、そして時々におけるロボットの姿勢を略線的に示す図を併せて示すものである。
【図２６】図２７乃至図２９とともにＰＴＰ動作からトラッキング動作への状態遷移について説明するための図であり、本図は諸量の定義について示すｔ−θ図である。
【図２７】ＰＴＰ動作時の作用端の移動先にトラッキング動作の対象となる移動物体が存在し、かつトラッキング動作への移行開始時点で作用端の位置がＰＴＰ動作に係るランプ信号の傾斜部上にある場合について、Δθtra、Δθtrbの各々の状況について生成されるランプ信号を示すｔ−θ図である。
【図２８】ＰＴＰ動作時の作用端の移動先にトラッキング動作の対象となる移動物体が存在し、かつトラッキング動作への移行開始時点で作用端の位置がＰＴＰ動作に係るランプ信号の平坦部上にある場合において生成されるランプ信号を示すｔ−θ図である。
【図２９】ＰＴＰ動作時の作用端の移動先とは反対の方向にトラッキング動作の対象となる移動物体が存在する場合において生成されるランプ信号を示すｔ−θ図である。
【符号の説明】
１  サーボ制御装置
２  指令手段
３  移動物体検出手段
５  制御指令値計算手段
６  ピーク速度到達時間値決定手段
７  ランプ折れ時間値決定手段
８  変調信号生成手段
９  時間軸伸縮用パラメータ値決定手段
１０  捕捉／近接判定手段
１１  加減速パターン生成手段
１２  切換手段
１３  制御対象
１４  ロボット制御装置
２３、２４  アーム
２５  ツール搭載部
【図１】
0003648812.tif 000039
【図２】
0003648812.tif 000040
【図３】
0003648812.tif 000041
【図４】
0003648812.tif 000042
【図５】
0003648812.tif 000043
【図６】
0003648812.tif 000044
【図７】
0003648812.tif 000045
【図８】
0003648812.tif 000046
【図９】
0003648812.tif 000047
【図１０】
0003648812.tif 000048
【図１１】
0003648812.tif 000049
【図１２】
0003648812.tif 000050
【図１３】
0003648812.tif 000051
【図１４】
0003648812.tif 000052
【図１５】
0003648812.tif 000053
【図１６】
0003648812.tif 000054
【図１７】
0003648812.tif 000055
【図１８】
0003648812.tif 000056
【図１９】
0003648812.tif 000057
【図２０】
0003648812.tif 000058
【図２１】
0003648812.tif 000059
【図２２】
0003648812.tif 000060
【図２３】
0003648812.tif 000061
【図２４】
0003648812.tif 000062
【図２５】
0003648812.tif 000063
【図２６】
0003648812.tif 000064
【図２７】
0003648812.tif 000065
【図２８】
0003648812.tif 000066
【図２９】
0003648812.tif 000067