JP3512651B2 - ロボットの制御装置および制御方法 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、ロボットの制御装
置および制御方法に関し、特にロボットの近傍通過制御
を行う制御装置および制御方法に関する。

【０００２】

【従来の技術】図１５には、３つの関節Ｊ１，Ｊ２，Ｊ
３を有する水平多関節ロボットの一例が示されている。
このロボットの第１関節Ｊ１はＸＹ平面の原点に固定さ
れている。また、第１関節Ｊ１には第１アームＡ１の一
端が連結されており、それによってこの第１アームＡ１
は第１関節Ｊ１を中心としてＸＹ平面において回動自在
になっている。第１アームＡ１の他端は第２関節Ｊ２に
連結されている。この第２関節Ｊ２には第２アームＡ２
の一端が連結されており、それによってこの第２アーム
Ａ２は第２関節Ｊ２を中心としてＸＹ平面において回動
自在になっている。そして第２アームＡ２の他端は第３
関節Ｊ３に連結され、さらに第３関節Ｊ３にはハンドＨ
の基端が連結されている。それによってハンドＨは第３
関節Ｊ３を中心としてＸＹ平面において回動自在になっ
ている。

【０００３】各関節Ｊ１，Ｊ２，Ｊ３にはそれぞれ独立
して駆動されるサーボモータ（図示省略）が連結されて
おり、各関節Ｊ１，Ｊ２，Ｊ３が各サーボモータにより
適宜回転されることにより、ロボットが自由にＸＹ平面
上を動くようになっている。

【０００４】図１６には、一例としてＸＹ平面上におい
て４ポイントＰ１，Ｐ２，Ｐ３，Ｐ４をこの順序で通る
ロボットの運転経路が示されている。そして図１７、図
１８、図１９および図２０には、それぞれ図１５に示す
ロボットの第３関節Ｊ３の軸が図１６に示す運転経路に
沿って動く場合のその第３関節Ｊ３の軸が各ポイントＰ
１，Ｐ２，Ｐ３，Ｐ４に位置する時のロボットの姿勢が
示されている。

【０００５】図２１には、Ｐ１からＰ２へ至る区間、Ｐ
２からＰ３へ至る区間、およびＰ３からＰ４へ至る区間
におけるハンドＨの先端のＸ軸方向およびＹ軸方向の速
度および加速度の変化の様子が示されている。ただしＰ
１とＰ２との間、Ｐ２とＰ３との間およびＰ３とＰ４と
の間をそれぞれ直線補間で補間し、各移動区間において
加速および減速を行っている。図２１において、曲線イ
および曲線ロはそれぞれＰ１−Ｐ２区間におけるＸ軸方
向（図１６参照）に対する速度および加速度の変化であ
り、曲線ハおよび曲線ニはそれぞれＰ２−Ｐ３区間にお
けるＹ軸方向（図１６参照）に対する速度および加速度
の変化であり、曲線ホおよび曲線ヘはそれぞれＰ３−Ｐ
４区間におけるＸ軸方向（図１６参照）に対する速度お
よび加速度の変化である。

【０００６】このような運転の場合には、図２１よりハ
ンドＨの先端の速度および加速度は、いずれもなめらか
に変化していることがわかる。しかしＰ２およびＰ３で
ハンドＨが一旦停止するため、Ｐ１からＰ２およびＰ３
を経てＰ４へ至るまでの運転時間が長くなってしまう。

【０００７】そこで、通常、図２２のようにＰ２および
Ｐ３を通らずにそれらの近傍を通過するような運転が行
われる。これは、ロボットの基準となる箇所（以下、基
準点とする）が予め指定された近傍範囲（図２２におい
て破線で示された円の範囲）内に入ると、現在の移動区
間の残距離の減速処理とつぎの移動区間の加速処理を同
時に行うように制御することによって達成される。この
近傍通過制御によれば、Ｐ２およびＰ３において一時停
止しないので運転時間が短縮される。

【０００８】図２３には、このような多関節ロボットの
駆動制御を行う駆動制御装置の構成が示されている。こ
の駆動制御装置は制御装置１と駆動装置２とからなる。

【０００９】制御装置１は、演算処理装置（以下、ＣＰ
Ｕとする）１１、プログラム・位置決めポイント保存用
メモリ１２、演算用ワークメモリ１３、Ｏ／Ｓメモリ１
４、入出力インターフェース１５およびサーボアンプイ
ンターフェース１６を備えており、それらがバスを介し
て相互に接続されている。

【００１０】プログラム・位置決めポイント保存用メモ
リ１２は、ランダムアクセスメモリ（以下、ＲＡＭとす
る）等からなり、入出力インターフェース１５を介して
制御装置１に外部より入力されたロボットの動作プログ
ラムや位置決めポイント等を記憶する。

【００１１】ＣＰＵ１１は、プログラム・位置決めポイ
ント保存用メモリ１２に記憶された動作プログラムおよ
び位置決めポイントに基づいてロボットの各関節軸の位
置の算出など種々の演算を行う。

【００１２】演算用ワークメモリ１３は、ＲＡＭ等から
なる作業用のメモリであり、ＣＰＵ１１により算出され
た各関節軸の位置等や一時的なデータを記憶する。

【００１３】Ｏ／Ｓメモリ１４は、読出し専用メモリ
（以下、ＲＯＭとする）等からなりＣＰＵ１１を動作さ
せるためのプログラムを記憶する。

【００１４】入出力インターフェース１５は、ロボット
の動作プログラムや位置決めポイント等を入力するため
の図示しない入力装置（キーボードやマウス等）や、図
示しない出力装置が接続されるインターフェースであ
る。

【００１５】サーボアンプインターフェース１６は、駆
動装置２に対して指令位置等の制御情報の通信を行うイ
ンターフェースである。

【００１６】また、駆動装置２は、第１関節Ｊ１を駆動
するための第１サーボアンプ２１およびそれに接続され
た第１モータ２６と、第２関節Ｊ２を駆動するための第
２サーボアンプ２２およびそれに接続された第２モータ
２７と、第３関節Ｊ３を駆動するための第３サーボアン
プ２３およびそれに接続された第３モータ２８と、を有
している。

【００１７】そしてＣＰＵ１１は、プログラム・位置決
めポイント保存用メモリ１２に記憶されている通過ポイ
ント、終点のポイントおよび速度等のデータを用いて、
ロボットの各関節軸の位置を微少時間ごとに演算して求
め、ロボットの現在位置として演算用ワークメモリ１３
へ記憶し、また、サーボアンプイターフェース１６を介
して駆動装置２へ位置指令情報として出力する。各サー
ボアンプ２１，２２，２３は、制御装置１のサーボアン
プインターフェース１６を介してその位置指令情報を受
け取り、その制御情報に基づいて各モータ２６，２７，
２８を回転させる。それによって各モータ２６，２７，
２８に接続されたロボットの各関節Ｊ１，Ｊ２，Ｊ３が
回転し、ロボットが指令座標に従った運転を行う。

【００１８】図２４には、従来の多関節ロボット駆動制
御装置の機能構成を示すブロック図が示されている。こ
の駆動制御装置は、残距離演算部３１、残距離判定部３
２、フラグ制御部３３、関節終点記憶実行部４１、停止
時間演算部４２、関節残距離演算部４３、関節速度演算
部４４、速度演算部４５、関節現在位置演算部４６およ
び速度制御部４７を備えている。これら各機能部は、Ｃ
ＰＵ１１が、プログラム・位置決めポイント保存用メモ
リ１２に記憶された動作プログラムに従って動作するこ
とによって実現される。

【００１９】残距離演算部３１は、ロボットの基準点の
現在位置と現在の移動区間の終点位置とに基づいて、現
在の移動区間における残距離を算出する。この時の現在
位置に関するデータは演算用ワークメモリ１３から得ら
れ、残距離演算部３１に入力される。また、終点位置に
関するデータはプログラム・位置決めポイント保存用メ
モリ１２から得られる。

【００２０】残距離判定部３２は、残距離演算部３１に
より算出された残距離と予め指定された近傍量（例えば
近傍範囲を特定する円の半径）とを比較し、残距離が近
傍量よりも小さい場合には、ロボットの基準点が近傍範
囲内に入ったと判断して近傍通過制御モードに移行し、
一方残距離が近傍量よりも大きい場合には、近傍通過制
御モードに移行しない。比較対象となる所定の近傍量に
関するデータはプログラム・位置決めポイント保存用メ
モリ１２から得られる。

【００２１】フラグ制御部３３は、近傍通過制御モード
に移行した場合に残距離判定部３２から入力する近傍通
過制御モード移行信号（制御信号）に基づいて、近傍通
過制御中を表すフラグを立てる。また、フラグ制御部３
３は、近傍通過制御が終了して通常モードに復帰した場
合にはフラグを降ろす。フラグ領域１３ａは例えば演算
用ワークメモリ１３内に割り当てられている。

【００２２】速度演算部４５は、ロボットの基準点の現
在位置と、制御装置１が各関節軸の座標を計算する周期
（通常数ｍ秒〜数十ｍ秒）（以下、この周期を補間周期
とする）の１周期前の位置（前回位置）との変位量、す
なわちロボットの基準点の１周期当たりの移動量に基づ
いて、速度を求める。この時の現在位置に関するデータ
および前回位置に関するデータは、例えば演算用ワーク
メモリ１３から得られる。

【００２３】関節終点記憶実行部４１は、残距離判定部
３２から入力する近傍通過制御モード移行信号をトリガ
ーとして、現在の移動区間における各関節軸の終点を演
算用ワークメモリ１３に記憶する。この時の各関節軸の
終点に関するデータは、現在の位置決めポイント終点を
関節軸終点に変換する演算をすることから得られ、関節
終点記憶実行部４１に入力される。

【００２４】停止時間演算部４２は、残距離判定部３２
から入力する近傍通過制御モード移行信号をトリガーと
して、残距離演算部３１により算出された残距離と、速
度演算部４５により得られた速度に基づいて、現在移動
中の区間における移動が停止するのに要する停止時間、
すなわち、速度がゼロになるのに要する時間を求める。

【００２５】関節現在位置演算部４６は、各関節軸の現
在位置を表す値を、補間周期で決められた時間ごとに演
算して求める。

【００２６】関節残距離演算部４３は、残距離判定部３
２から入力する近傍通過制御モード移行信号をトリガー
として、現在の移動区間における各関節軸の終点と、関
節現在位置演算部４６により求められた各関節軸の現在
位置との差に基づいて、各関節軸の現在の移動区間にお
ける残距離を求める。この時の各関節軸の終点に関する
データは、現在の位置決めポイント終点を関節軸終点に
変換する演算をすることから得られる。

【００２７】関節速度演算部４４は、残距離判定部３２
から入力する近傍通過制御モード移行信号をトリガーと
して、関節現在位置演算部４６により求められた各関節
軸の現在位置と、補間周期の１周期前の各関節軸の位置
（前回位置）との変位量、すなわち各関節軸の１周期当
たりの移動量に基づいて、速度を求める。この時の前回
位置に関するデータは例えば演算用ワークメモリ１３か
ら得られる。

【００２８】停止時間演算部４２、関節残距離演算部４
３および関節速度演算部４４のそれぞれの演算結果、す
なわち各関節軸の現在の移動区間における停止時間、各
関節軸の残距離および各関節の速度は演算用ワークメモ
リ１３に記憶される。

【００２９】速度制御部４７は、関節速度演算部４４に
より求められた近傍通過制御処理開始時の関節軸の速
度、および停止時間演算部４２により求められた停止時
間等に基づいて、近傍通過制御処理の実行中に関節軸の
速度の制御を行う。

【００３０】図２５には、従来の近傍通過制御を開始す
るか否かを判断する処理のフローチャートが示されてい
る。この処理が開始されると、ＣＰＵ１１はまずステッ
プＳ１１でロボットの基準点に対して、その基準点の現
在位置と現在の移動区間の終点位置とに基づいて、現在
の移動区間における残距離を計算し、次いでステップＳ
１２でその残距離と予め指定された近傍量とを比較す
る。残距離が近傍量よりも小さい場合には、ＣＰＵ１１
はロボットの基準点が近傍範囲内に入ったと判断し、ス
テップＳ１３へ進んで近傍通過制御を開始する。ステッ
プＳ１３ではＣＰＵ１１は近傍通過制御開始時に所定の
データ（以下、近傍開始時データとする）を作成し（こ
のデータ作成については後述する）、ステップＳ１４に
てＣＰＵ１１は近傍通過制御中を示すフラグを立て、近
傍通過制御処理中となり、次いで処理を終了する。ステ
ップＳ１２で残距離が近傍量よりも大きい場合には、近
傍通過制御を開始せずに処理を終了する。

【００３１】図２６および図２７には、それぞれ近傍通
過制御時のＰ３近傍におけるロボットの基準点の経路が
示されている。Ｐ２からＰ３へ至る区間で近傍範囲に入
った時の速度と、Ｐ３からＰ４へ至る区間で近傍範囲か
ら出た時の速度とが同じで、かつＰ２からＰ３へ至る区
間における近傍範囲内の減速時間と、Ｐ３からＰ４へ至
る区間における近傍範囲内の加速時間とが同じである場
合には、ロボットの基準点は図２６に示すように４分の
１円の円弧上の経路を通過する。それに対してＰ２から
Ｐ３へ至る区間における近傍範囲内の減速時間が、Ｐ３
からＰ４へ至る区間における近傍範囲内の加速時間より
も長い場合には、ロボットの基準点は図２７に示すよう
に近傍通過制御処理の終了時点で近傍範囲を示す円の外
に出てしまっている。すなわち、近傍範囲内で近傍通過
制御処理が終了しなくなってしまう。

【００３２】図２８は、図２５のステップＳ１３におけ
る近傍通過制御開始時のデータ作成処理の詳細な手順を
説明するフローチャートである。このデータ作成処理が
開始されると、まずステップＳ２１でＣＰＵ１１は現在
の移動区間における各関節軸の終点を演算用ワークメモ
リ１３に記憶する。続いてステップＳ２２でＣＰＵ１１
は次式に基づいて現在の移動区間における停止時間を求
める。 （停止までの時間）＝（補間残距離）／（補間速度）

【００３３】続いてステップＳ２３でＣＰＵ１１は次式
に基づいて各関節軸の残距離を求める。 （関節の残距離）＝（関節の終点）−（関節の今回位
置）

【００３４】続いてステップＳ２４でＣＰＵ１１は次式
に基づいて各関節の速度を求め、それからデータ作成処
理を終了する。なお各関節軸の値は、補間周期で決めら
れた時間ごとに演算されて求められるため、ここでは現
在の値を今回位置と呼ぶ。 （関節の速度）＝（関節の前回の位置）−（関節の今回
位置）

【００３５】図２９には、速度制御部４７による従来の
近傍通過制御時における関節軸の速度の変化が示されて
いる。なお図２９のグラフは、移動中の区間の残距離成
分の速度の変化であり、つぎの移動区間すなわち近傍通
過後の移動区間の加速成分を含まない。従って関節軸
は、直線トに示すように、図２８のステップＳ２４で求
めた近傍通過制御開始時の関節の速度から直線的に減速
する。図２９において近傍の減速時間Ｔ１は、図２８の
ステップＳ２２で求めた停止までの時間である。直線ト
で示す直線的な減速で移動する距離と、図２８のステッ
プＳ２３で求めた残距離との差分は、曲線チのような三
角形の速度パターンを用いて補正する。そして直線減速
グラフすなわち直線トと、残距離補正グラフすなわち曲
線チを加算することにより、近傍通過制御処理による関
節軸の合成速度の変化を示す曲線リが得られる。近傍通
過制御における減速処理では、各関節軸ごとに図２９に
示す減速処理が行われる。すなわち近傍通過制御の開始
前は合成速度をなめらかに制御しているが、近傍通過制
御処理中は各軸ごとに直線加速・減速処理に切り替わ
る。

【００３６】図３０には、図２２に示すポイントＰ２を
近傍通過制御しながら通過した場合の、ハンド先端のＸ
軸方向およびＹ軸方向の速度と加速度の変化が示されて
いる。図３０において、曲線ヌおよび曲線ルはそれぞれ
Ｘ軸方向（図２２参照）に対する速度および加速度の変
化であり、曲線ヲおよび曲線ワはそれぞれＹ軸方向（図
２２参照）に対する速度および加速度の変化である。こ
の図より近傍通過制御開始と同時にＹ軸方向の加速が始
まっていることと、近傍通過制御処理が終了する時にＸ
軸方向の加速度が急に変化していることが分かる。

【００３７】

【発明が解決しようとする課題】上述したように従来の
近傍通過制御における指令値が生成されるため、従来
は、図２２に示すような近傍通過制御において、近傍通
過をはさんだ前の移動区間の減速時間が長く、後ろの移
動区間の加速時間が短い場合に、図２７に示すように近
傍通過制御の終了が近傍範囲を特定する円の外に出てし
まう場合があった。従って移動の軌跡を重視するような
場合には、近傍範囲内で近傍通過制御を終了させるた
め、加速および減速時間を移動プログラムごとに調整し
なければならず、非常に手間がかかるという問題点があ
った。

【００３８】また、近傍通過制御処理中の残距離の出力
制御を図２９の減速パターンで行っているため、補間方
法が切り替わる近傍通過制御の開始時と、残距離補正が
反転する曲線チの三角形の頂点時と、近傍通過制御の終
了時の３箇所において加速度が急激に変化し、ロボット
の関節軸やアームやハンドに振動が発生するという問題
点もあった。

【００３９】本発明は、上記問題点を解決するためにな
されたもので、近傍通過制御処理の終了時にロボットの
基準点が近傍範囲外に出ないように自動的に制御され、
それによって近傍通過制御処理に伴う移動プログラムの
微調整の軽減を図ることができるロボットの制御装置お
よび制御方法を得ることを目的とする。

【００４０】また、近傍通過制御処理時における加速度
の乱れが殆どなく、従ってロボットをなめらかに動かし
得るロボットの制御装置および制御方法を得ることを目
的とする。

【００４１】

【課題を解決するための手段】上記目的を達成するた
め、本発明は、ロボットの基準点が通過するように予め
指定された通過点に対して、該通過点の直前の移動区間
における減速と該通過点の直後の移動区間における加速
とを同時に行うことによって、該通過点を通らずにその
通過点の近傍を通過するようにロボットの動作を制御す
る近傍通過制御処理を行う制御装置であって、通過点の
通過前に前記基準点の現在の移動区間における残距離を
求める残距離演算部と、該残距離演算部により求められ
た残距離を所定の近傍量と比較する残距離判定部と、前
記残距離が前記近傍量よりも小さい場合にのみ、前記基
準点の、現在の移動区間における移動方向の成分の移動
が停止するのに要する停止時間を求める停止時間演算部
と、該停止時間演算部により求められた停止時間内に前
記基準点が移動すると想定される距離のうち、つぎの移
動区間の進行方向の成分の距離を演算して求める次区間
進行距離演算部と、該次区間進行距離演算部により求め
られた進行方向成分の距離を所定の近傍量と比較し、前
記進行方向成分の距離が前記近傍量よりも小さい場合に
のみ、前記近傍通過制御処理を開始する次区間進行距離
判定部と、を具備することを特徴とする。

【００４２】この発明によれば、通過点の通過前に現在
の移動区間における残距離が求められ、その残距離が所
定の近傍量と比較される。そしてその残距離が近傍量よ
りも小さい場合に、現在の移動区間における移動方向の
成分の移動が停止するのに要する停止時間が求められ、
その停止時間内に想定される移動距離のうち、つぎの移
動区間の進行方向の成分の距離が求められる。その進行
方向成分の距離は所定の近傍量と比較され、進行方向成
分の距離が近傍量よりも小さい場合に、近傍通過制御処
理が開始される。

【００４３】また、上記目的を達成するため、本発明
は、少なくとも１つの移動自在な関節を有するロボット
の基準点が通過するように予め指定された通過点に対し
て、該通過点の直前の移動区間における減速と該通過点
の直後の移動区間における加速とを同時に行うことによ
って、該通過点を通らずにその通過点の近傍を通過する
ようにロボットの動作を制御する近傍通過制御処理を行
う制御装置であって、前記近傍通過制御処理開始時の前
記基準点の、現在の移動区間における移動方向の成分の
移動が停止するのに要する停止時間を求める停止時間演
算部と、前記近傍通過制御処理時に現在の移動区間にお
ける前記各関節の現在位置から終点までの残距離を求め
る関節残距離演算部と、前記近傍通過制御処理時に前記
各関節の速度を求める関節速度演算部と、前記近傍通過
制御処理時に前記各関節の加速度を求める関節加速度演
算部と、前記近傍通過制御処理開始時の停止時間、該近
傍通過制御処理開始時の関節の移動残距離、該近傍通過
制御処理開始時の関節の速度、および該近傍通過制御処
理開始時の関節の加速度に基づいて決まる近傍減速定数
を算出する関節近傍減速定数演算部と、前記近傍通過制
御処理の実行中の関節の加速度が前記停止時間中になめ
らかにゼロになるような第１の位置制御値を、前記近傍
減速定数を用いて演算して求める加速度制御部と、前記
第１の位置制御値に基づいて前記近傍通過制御処理の実
行中に変化する関節の速度を考慮して、該近傍通過制御
処理中の関節の速度が前記停止時間中になめらかにゼロ
になるような第２の位置制御値を、前記近傍減速定数を
用いて演算して求める速度制御部と、前記第１の位置制
御値および前記第２の位置制御値に基づいて不足する関
節の移動量がなめらかに変化するような第３の位置制御
値を、前記近傍減速定数を用いて演算して求める残距離
制御部と、前記第１の位置制御値、前記第２の位置制御
値および前記第３の位置制御値を合計して前記近傍通過
制御処理中の関節の位置を求める加算部と、を具備する
ことを特徴とする。

【００４４】この発明によれば、近傍通過制御開始時の
現在の移動区間における移動方向の成分の移動が停止す
るのに要する停止時間、近傍通過制御開始時の現在の移
動区間における各関節の現在位置から終点までの残距
離、近傍通過制御開始時の各関節の速度、および近傍通
過制御開始時の各関節の加速度が求められる。そして近
傍通過制御開始時の停止時間、近傍通過制御開始時の関
節の移動残距離、近傍通過制御開始時の関節の速度、お
よび近傍通過制御開始時の関節の加速度に基づいて決ま
る定数が求められる。

【００４５】

【００４６】また、この発明によれば、近傍通過制御処
理の実行中の関節の加速度が停止時間中になめらかにゼ
ロになるような第１の位置制御値、その第１の位置制御
値に基づいて近傍通過制御処理の実行中に変化する関節
の速度を考慮して、近傍通過制御処理中の関節の速度が
停止時間中になめらかにゼロになるような第２の位置制
御値、並びに第１の位置制御値および第２の位置制御値
に基づいて不足する関節の移動量がなめらかに変化する
ような第３の位置制御値が求められる。そして第１の位
置制御値、第２の位置制御値および第３の位置制御値を
合計することにより近傍通過制御処理中の関節の位置が
求められる。

【００４７】さらにこの発明において、前記第１の位置
制御値、前記第２の位置制御値および前記第３の位置制
御値は、何れも三角関数で表されてもよい。

【００４８】この発明によれば、第１の位置制御値、第
２の位置制御値および第３の位置制御値は、何れも三角
関数で表される。

【００４９】また、上記目的を達成するため、本発明
は、少なくとも１つの移動自在な関節を有するロボット
の基準点が通過するように予め指定された通過点に対し
て、該通過点の直前の移動区間における減速と該通過点
の直後の移動区間における加速とを同時に行うことによ
って、該通過点を通らずにその通過点の近傍を通過する
ようにロボットの動作を制御する近傍通過制御処理を行
う制御装置であって、通過点の通過前に前記基準点の現
在の移動区間における残距離を求める残距離演算部と、
該残距離演算部により求められた残距離を所定の近傍量
と比較する残距離判定部と、前記残距離が前記近傍量よ
りも小さい場合または近傍通過制御開始時に、前記基準
点の、現在の移動区間における移動方向の成分の移動が
停止するのに要する停止時間を求める停止時間演算部
と、前記残距離が前記近傍量よりも小さい場合における
該停止時間演算部により求められた停止時間内に前記基
準点が移動すると想定される距離のうち、つぎの移動区
間の進行方向の成分の距離を演算して求める次区間進行
距離演算部と、該次区間進行距離演算部により求められ
た進行方向成分の距離を所定の近傍量と比較し、前記進
行方向成分の距離が前記近傍量よりも小さい場合にの
み、前記近傍通過制御処理を開始する次区間進行距離判
定部と、前記近傍通過制御処理時に現在の移動区間にお
ける前記各関節の現在位置から終点までの残距離を求め
る関節残距離演算部と、前記近傍通過制御処理時に前記
各関節の速度を求める関節速度演算部と、前記近傍通過
制御処理時に前記各関節の加速度を求める関節加速度演
算部と、前記近傍通過制御処理開始時の停止時間、該近
傍通過制御処理開始時の関節の移動残距離、該近傍通過
制御処理開始時の関節の速度、および該近傍通過制御処
理開始時の関節の加速度に基づいて決まる近傍減速定数
を算出する関節近傍減速定数演算部と、前記近傍通過制
御処理の実行中の関節の加速度が前記近傍通過制御処理
開始時の停止時間中になめらかにゼロになるような第１
の位置制御値を、前記近傍減速定数を用いて演算して求
める加速度制御部と、前記第１の位置制御値に基づいて
前記近傍通過制御処理の実行中に変化する関節の速度を
考慮して、該近傍通過制御処理中の関節の速度が前記近
傍通過制御処理開始時の停止時間中になめらかにゼロに
なるような第２の位置制御値を、前記近傍減速定数を用
いて演算して求める速度制御部と、前記第１の位置制御
値および前記第２の位置制御値に基づいて不足する関節
の移動量がなめらかに変化するような第３の位置制御値
を、前記近傍減速定数を用いて演算して求める残距離制
御部と、前記第１の位置制御値、前記第２の位置制御値
および前記第３の位置制御値を合計して前記近傍通過制
御処理中の関節の位置を求める加算部と、を具備するこ
とを特徴とする。

【００５０】この発明によれば、通過点の通過前に現在
の移動区間における残距離が求められ、その残距離が所
定の近傍量と比較され、残距離が近傍量よりも小さい場
合に、現在の移動区間における移動方向の成分の移動が
停止するのに要する停止時間が求められ、その停止時間
内に想定される移動距離のうち、つぎの移動区間の進行
方向の成分の距離が求められる。そしてその進行方向成
分の距離は所定の近傍量と比較され、進行方向成分の距
離が近傍量よりも小さい場合に、近傍通過制御処理が開
始される。近傍通過制御処理が開始されると、近傍通過
制御開始時の現在の移動区間における移動方向の成分の
移動が停止するのに要する停止時間、近傍通過制御開始
時の現在の移動区間における各関節の現在位置から終点
までの残距離、近傍通過制御開始時の各関節の速度、お
よび近傍通過制御開始時の各関節の加速度が求められ
る。そして近傍通過制御開始時の停止時間、近傍通過制
御開始時の関節の移動残距離、近傍通過制御開始時の関
節の速度、および近傍通過制御開始時の関節の加速度に
基づいて決まる定数が求められる。また、近傍通過制御
処理の実行中の関節の加速度が停止時間中になめらかに
ゼロになるような第１の位置制御値、その第１の位置制
御値に基づいて近傍通過制御処理の実行中に変化する関
節の速度を考慮して、近傍通過制御処理中の関節の速度
が停止時間中になめらかにゼロになるような第２の位置
制御値、並びに第１の位置制御値および第２の位置制御
値に基づいて不足する関節の移動量がなめらかに変化す
るような第３の位置制御値が求められる。そして第１の
位置制御値、第２の位置制御値および第３の位置制御値
を合計することにより近傍通過制御処理中の関節の位置
が求められる。

【００５１】また、上記目的を達成するため、本発明
は、ロボットの基準点が通過するように予め指定された
通過点に対して、該通過点の直前の移動区間における減
速と該通過点の直後の移動区間における加速とを同時に
行うことによって、該通過点を通らずにその通過点の近
傍を通過するようにロボットの動作を制御する近傍通過
制御処理を行う制御方法において、通過点の通過前に前
記基準点の現在の移動区間における残距離を求める残距
離演算部と、該残距離演算部により求められた残距離を
所定の近傍量と比較する工程と、前記残距離が前記近傍
量よりも小さい場合にのみ、前記基準点の、現在の移動
区間における移動方向の成分の移動が停止するのに要す
る停止時間を求める工程と、該停止時間演算部により求
められた停止時間内に前記基準点が移動すると想定され
る距離のうち、つぎの移動区間の進行方向の成分の距離
を演算して求める工程と、該次区間進行距離演算部によ
り求められた進行方向成分の距離を所定の近傍量と比較
し、前記進行方向成分の距離が前記近傍量よりも小さい
場合にのみ、前記近傍通過制御処理を開始する工程と、
を含むことを特徴とする。

【００５２】この発明によれば、通過点の通過前に現在
の移動区間における残距離が求められ、その残距離が所
定の近傍量と比較される。そしてその残距離が近傍量よ
りも小さい場合に、現在の移動区間における移動方向の
成分の移動が停止するのに要する停止時間が求められ、
その停止時間内に想定される移動距離のうち、つぎの移
動区間の進行方向の成分の距離が求められる。その進行
方向成分の距離は所定の近傍量と比較され、進行方向成
分の距離が近傍量よりも小さい場合に、近傍通過制御処
理が開始される。

【００５３】また、上記目的を達成するため、本発明
は、少なくとも１つの移動自在な関節を有するロボット
の基準点が通過するように予め指定された通過点に対し
て、該通過点の直前の移動区間における減速と該通過点
の直後の移動区間における加速とを同時に行うことによ
って、該通過点を通らずにその通過点の近傍を通過する
ようにロボットの動作を制御する近傍通過制御処理を行
う制御方法において、前記近傍通過制御処理開始時の前
記基準点の、現在の移動区間における移動方向の成分の
移動が停止するのに要する停止時間を求める工程と、前
記近傍通過制御処理時に現在の移動区間における前記各
関節の現在位置から終点までの残距離を求める工程と、
前記近傍通過制御処理時に前記各関節の速度を求める工
程と、前記近傍通過制御処理時に前記各関節の加速度を
求める工程と、前記近傍通過制御処理開始時の停止時
間、該近傍通過制御処理開始時の関節の移動残距離、該
近傍通過制御処理開始時の関節の速度、および該近傍通
過制御処理開始時の関節の加速度に基づいて決まる近傍
減速定数を算出する工程と、前記近傍通過制御処理の実
行中の関節の加速度が前記停止時間中になめらかにゼロ
になるような第１の位置制御値を、前記近傍減速定数を
用いて演算して求める工程と、前記第１の位置制御値に
基づいて前記近傍通過制御処理の実行中に変化する関節
の速度を考慮して、該近傍通過制御処理中の関節の速度
が前記停止時間中になめらかにゼロになるような第２の
位置制御値を、前記近傍減速定数を用いて演算して求め
る工程と、前記第１の位置制御値および前記第２の位置
制御値に基づいて不足する関節の移動量がなめらかに変
化するような第３の位置制御値を、前記近傍減速定数を
用いて演算して求める工程と、前記第１の位置制御値、
前記第２の位置制御値および前記第３の位置制御値を合
計して前記近傍通過制御処理中の関節の位置を求める工
程と、を含むことを特徴とする。

【００５４】この発明によれば、近傍通過制御開始時の
現在の移動区間における移動方向の成分の移動が停止す
るのに要する停止時間、近傍通過制御開始時の現在の移
動区間における各関節の現在位置から終点までの残距
離、近傍通過制御開始時の各関節の速度、および近傍通
過制御開始時の各関節の加速度が求められる。そして近
傍通過制御開始時の停止時間、近傍通過制御開始時の関
節の移動残距離、近傍通過制御開始時の関節の速度、お
よび近傍通過制御開始時の関節の加速度に基づいて決ま
る定数が求められる。

【００５５】

【００５６】また、この発明によれば、近傍通過制御処
理の実行中の関節の加速度が停止時間中になめらかにゼ
ロになるような第１の位置制御値、その第１の位置制御
値に基づいて近傍通過制御処理の実行中に変化する関節
の速度を考慮して、近傍通過制御処理中の関節の速度が
停止時間中になめらかにゼロになるような第２の位置制
御値、並びに第１の位置制御値および第２の位置制御値
に基づいて不足する関節の移動量がなめらかに変化する
ような第３の位置制御値が求められる。そして第１の位
置制御値、第２の位置制御値および第３の位置制御値を
合計することにより近傍通過制御処理中の関節の位置が
求められる。

【００５７】さらにこの発明において、前記第１の位置
制御値、前記第２の位置制御値および前記第３の位置制
御値は、何れも三角関数で表されてもよい。

【００５８】この発明によれば、第１の位置制御値、第
２の位置制御値および第３の位置制御値は、何れも三角
関数で表される。

【００５９】また、上記目的を達成するため、本発明
は、少なくとも１つの移動自在な関節を有するロボット
の基準点が通過するように予め指定された通過点に対し
て、該通過点の直前の移動区間における減速と該通過点
の直後の移動区間における加速とを同時に行うことによ
って、該通過点を通らずにその通過点の近傍を通過する
ようにロボットの動作を制御する近傍通過制御処理を行
う制御方法において、通過点の通過前に前記基準点の現
在の移動区間における残距離を求める工程と、該残距離
演算部により求められた残距離を所定の近傍量と比較す
る工程と、前記残距離が前記近傍量よりも小さい場合ま
たは近傍通過制御開始時に、前記基準点の、現在の移動
区間における移動方向の成分の移動が停止するのに要す
る停止時間を求める工程と、前記残距離が前記近傍量よ
りも小さい場合における該停止時間演算部により求めら
れた停止時間内に前記基準点が移動すると想定される距
離のうち、つぎの移動区間の進行方向の成分の距離を演
算して求める工程と、該次区間進行距離演算部により求
められた進行方向成分の距離を所定の近傍量と比較し、
前記進行方向成分の距離が前記近傍量よりも小さい場合
にのみ、前記近傍通過制御処理を開始する工程と、前記
近傍通過制御処理時に現在の移動区間における前記各関
節の現在位置から終点までの残距離を求める工程と、前
記近傍通過制御処理時に前記各関節の速度を求める工程
と、前記近傍通過制御処理時に前記各関節の加速度を求
める工程と、前記近傍通過制御処理開始時の停止時間、
該近傍通過制御処理開始時の関節の移動残距離、該近傍
通過制御処理開始時の関節の速度、および該近傍通過制
御処理開始時の関節の加速度に基づいて決まる近傍減速
定数を算出する工程と、前記近傍通過制御処理の実行中
の関節の加速度が前記近傍通過制御処理開始時の停止時
間中になめらかにゼロになるような第１の位置制御値
を、前記近傍減速定数を用いて演算して求める工程と、
前記第１の位置制御値に基づいて前記近傍通過制御処理
の実行中に変化する関節の速度を考慮して、該近傍通過
制御処理中の関節の速度が前記近傍通過制御処理開始時
の停止時間中になめらかにゼロになるような第２の位置
制御値を、前記近傍減速定数を用いて演算して求める工
程と、前記第１の位置制御値および前記第２の位置制御
値に基づいて不足する関節の移動量がなめらかに変化す
るような第３の位置制御値を、前記近傍減速定数を用い
て演算して求める工程と、前記第１の位置制御値、前記
第２の位置制御値および前記第３の位置制御値を合計し
て前記近傍通過制御処理中の関節の位置を求める工程
と、を含むことを特徴とする。

【００６０】この発明によれば、通過点の通過前に現在
の移動区間における残距離が求められ、その残距離が所
定の近傍量と比較され、残距離が近傍量よりも小さい場
合に、現在の移動区間における移動方向の成分の移動が
停止するのに要する停止時間が求められ、その停止時間
内に想定される移動距離のうち、つぎの移動区間の進行
方向の成分の距離が求められる。そしてその進行方向成
分の距離は所定の近傍量と比較され、進行方向成分の距
離が近傍量よりも小さい場合に、近傍通過制御処理が開
始される。近傍通過制御処理が開始されると、近傍通過
制御開始時の現在の移動区間における移動方向の成分の
移動が停止するのに要する停止時間、近傍通過制御開始
時の現在の移動区間における各関節の現在位置から終点
までの残距離、近傍通過制御開始時の各関節の速度、お
よび近傍通過制御開始時の各関節の加速度が求められ
る。そして近傍通過制御開始時の停止時間、近傍通過制
御開始時の関節の移動残距離、近傍通過制御開始時の関
節の速度、および近傍通過制御開始時の関節の加速度に
基づいて決まる定数が求められる。また、近傍通過制御
処理の実行中の関節の加速度が停止時間中になめらかに
ゼロになるような第１の位置制御値、その第１の位置制
御値に基づいて近傍通過制御処理の実行中に変化する関
節の速度を考慮して、近傍通過制御処理中の関節の速度
が停止時間中になめらかにゼロになるような第２の位置
制御値、並びに第１の位置制御値および第２の位置制御
値に基づいて不足する関節の移動量がなめらかに変化す
るような第３の位置制御値が求められる。そして第１の
位置制御値、第２の位置制御値および第３の位置制御値
を合計することにより近傍通過制御処理中の関節の位置
が求められる。

【００６１】

【発明の実施の形態】以下に、本発明に係るロボットの
制御装置および制御方法の実施の形態について図面を参
照しつつ詳細に説明する。

【００６２】本発明に係る多関節ロボット駆動制御装置
のハードウェア構成は図２３に示す一般的な駆動制御装
置と同じである。すなわち本発明に係る駆動制御装置
は、例えばＣＰＵ１１、プログラム・位置決めポイント
保存用メモリ１２、演算用ワークメモリ１３、Ｏ／Ｓメ
モリ１４、入出力インターフェース１５およびサーボア
ンプインターフェース１６がバスを介して相互に接続さ
れてなる制御装置１と、例えば図１５に示す構成の多関
節ロボットに対して例えば第１関節Ｊ１を駆動するため
の第１サーボアンプ２１および第１モータ２６、例えば
第２関節Ｊ２を駆動するための第２サーボアンプ２２お
よび第２モータ２７、例えば第３関節Ｊ３を駆動するた
めの第３サーボアンプ２３および第３モータ２８を備え
てなる駆動装置２とからなる。各構成要素の詳細な説明
については、図２３について先に記載した説明と重複す
るので省略する。

【００６３】図１には、本発明に係る多関節ロボット駆
動制御装置の機能構成を示すブロック図が示されてい
る。この駆動制御装置は、図２４に示す従来の駆動制御
装置と同様に、残距離演算部３１、残距離判定部３２、
フラグ制御部３３、関節終点記憶実行部４１、停止時間
演算部４２、関節残距離演算部４３、関節速度演算部４
４、速度演算部４５および関節現在位置演算部４６を備
えている。これら従来装置と同様の各機能部の詳細な説
明については、図２４について先に記載した説明と重複
するので省略する。ただし関節終点記憶実行部４１、関
節残距離演算部４３および関節速度演算部４４は何れ
も、残距離判定部３２から入力する制御信号に対してで
はなく、次区間進行距離判定部５２から入力する近傍通
過制御モード移行信号をトリガーとして、各機能の実行
を開始する点と、停止時間演算部４２が残距離判定部３
２から入力する制御信号と次区間進行距離判定部５２か
ら入力する近傍通過制御モード移行信号の何れに対して
もそれらをトリガーとして、機能の実行を開始する点で
上記従来装置と異なる。

【００６４】また、この駆動制御装置は、上記各機能部
の他に、次区間進行距離演算部５１、次区間進行距離判
定部５２、関節加速度演算部５３、関節近傍減速定数演
算部５４、加速度制御部６１、速度制御部６２、残距離
制御部６３および加算部６４を備えている。

【００６５】図１に示された各機能部は、ＣＰＵ１１
が、プログラム・位置決めポイント保存用メモリ１２に
記憶された動作プログラムに従って動作することによっ
て実現される。なお従来同様、フラグ制御部３３によっ
て制御されるフラグの格納領域１３ａは演算用ワークメ
モリ１３内に設けられている。

【００６６】次区間進行距離演算部５１は、停止時間演
算部４２から出力される停止時間に基づいて、この停止
時間の間にロボットの基準点（例えばハンド先端）が移
動すると想定される距離のうち、つぎの移動区間におけ
る進行方向の成分の距離（次区間進行距離）を演算して
求め、それによって近傍通過制御処理の終了時に例えば
ハンド先端が到達していると想定される位置を得る。

【００６７】次区間進行距離判定部５２は、次区間進行
距離演算部５１により算出された次区間進行距離と予め
指定された近傍量（例えば近傍範囲を特定する円の半
径）とを比較し、次区間進行距離が近傍量よりも小さい
場合には、ロボットの基準点が次区間において近傍範囲
内にある内に近傍通過制御処理が終了すると判断して近
傍通過制御モードに移行し、一方次区間進行距離が近傍
量よりも大きい場合には、近傍通過制御モードに移行し
ない。比較対象となる所定の近傍量に関するデータはプ
ログラム・位置決めポイント保存用メモリ１２から得ら
れる。

【００６８】関節加速度演算部５３は、各関節軸の今回
の速度と、補間周期の１周期前の各関節軸の速度（前回
速度）との差に基づいて、加速度を求める。この時の今
回の速度および前回速度に関するデータは例えば演算用
ワークメモリ１３から得られる。

【００６９】関節近傍減速定数演算部５４は、停止時間
演算部４２から出力される停止時間に基づいて、各関節
軸に対して、近傍通過制御処理の実行中に近傍減速を行
う演算において使用される定数（以下、近傍減速定数と
する）を演算して求める。近傍減速定数については後述
する。

【００７０】停止時間演算部４２、関節残距離演算部４
３、関節速度演算部４４、関節加速度演算部５３および
関節近傍減速定数演算部５４のそれぞれの演算結果、す
なわち各関節軸の現在の移動区間における停止時間、各
関節軸の残距離、各関節の速度、各関節の加速度および
各関節の近傍減速定数は演算用ワークメモリ１３に記憶
される。

【００７１】加速度制御部６１は、関節加速度演算部５
３により求められた近傍通過制御処理開始時の関節軸の
加速度、および停止時間演算部４２により求められた停
止時間等に基づいて、近傍通過制御処理の実行中の関節
軸の加速度がその停止時間中になめらかにゼロになるよ
うな制御を行うために、位置制御値を演算して求める。

【００７２】速度制御部６２は、関節速度演算部４４に
より求められた近傍通過制御処理開始時の関節軸の速
度、および停止時間演算部４２により求められた停止時
間等に基づいて、加速度制御部６１による加速度制御に
伴って近傍通過制御処理の実行中に変化する関節軸の速
度を考慮して近傍通過制御処理中の関節軸の速度がその
停止時間中になめらかにゼロになるような制御を行うた
めに、別の位置制御値を演算して求める。

【００７３】残距離制御部６３は、関節残距離演算部４
３により求められた近傍通過制御処理開始時の関節軸の
残距離、および停止時間演算部４２により求められた停
止時間等に基づいて、加速度制御部６１による加速度制
御および速度制御部６２による速度制御に伴って不足す
る関節軸の移動量がなめらかに変化するような制御を行
うために、さらに別の位置制御値を演算して求める。

【００７４】加算部６４は、加速度制御部６１、速度制
御部６２および残距離制御部６３でそれぞれ求められた
各位置制御値を合算する。その合計された制御値はサー
ボアンプインターフェース１６へ出力される。

【００７５】図２には、本発明に係るロボット制御方法
において近傍通過制御を開始するか否かを判断する処理
の一例のフローチャートが示されている。この処理が開
始されると、ＣＰＵ１１はまずステップＳ３１でロボッ
トの基準点に対して、その基準点の現在位置と現在の移
動区間の終点位置とに基づいて、現在の移動区間におけ
る残距離を計算する。次いでステップＳ３２でＣＰＵ１
１はその残距離と予め指定された近傍量とを比較する。
残距離が近傍量よりも小さい場合には、ＣＰＵ１１はロ
ボットの基準点が近傍範囲内に入ったと判断し、ステッ
プＳ３３へ進む。一方ステップＳ３２で残距離が近傍量
よりも大きい場合には、本処理を終了する。

【００７６】ステップＳ３３では、ＣＰＵ１１は次式に
基づいて現在の移動区間における停止時間を求める。 （停止までの時間）＝（補間残距離）／（補間速度）

【００７７】続いてステップＳ３４において、ステップ
Ｓ３３で求めた停止時間の間に、つぎの移動区間におい
てロボットの基準点が進む距離を求め、それによって近
傍通過制御処理の終了時の次区間における例えばハンド
の先端位置を演算して得る。この演算結果は、加減速時
間、移動量および速度によって変化するため、一様では
ない。

【００７８】そしてステップＳ３５に進み、ステップＳ
３４で求めた次区間における進行距離と予め指定された
近傍量とを比較する。次区間進行距離が近傍量よりも小
さい場合には、ロボットの基準点が次区間の近傍範囲外
に出る前に近傍通過制御処理が終了すると判断して近傍
通過制御モードに移行する。一方次区間進行距離が近傍
量よりも大きい場合には、ロボットの基準点が次区間の
近傍範囲外に出た後に近傍通過制御処理が終了すると判
断して近傍通過制御モードに移行せずに本処理を終了す
る。

【００７９】ステップＳ３５で近傍通過制御モードに移
行すると、ステップＳ３６でＣＰＵ１１は近傍通過制御
開始時の所定のデータ（近傍開始時データ）を作成し
（このデータ作成については後述する）、ステップＳ３
７にてＣＰＵ１１は近傍通過制御中を示すフラグを立て
て本処理を終了する。

【００８０】このように、上記ステップＳ３３、ステッ
プＳ３４およびステップＳ３５の演算および比較処理に
より、近傍通過制御処理の開始タイミングが調整され
る。つまり例えば図２２のポイントＰ３に対して図３に
示すように近傍範囲を表す円内において近傍通過制御処
理が遅れて開始し、それによってロボットの基準点が近
傍範囲内に残っている間に近傍通過制御処理が終了す
る。

【００８１】図４は、図２のステップＳ３６における近
傍通過制御開始時のデータ作成処理の詳細な手順を説明
するフローチャートである。このデータ作成処理が開始
されると、まずステップＳ４１でＣＰＵ１１は現在の移
動区間における各関節軸の終点を演算用ワークメモリ１
３に記憶する。続いてステップＳ４２でＣＰＵ１１は次
式に基づいて現在の移動区間における停止時間を求め
る。 （停止までの時間）＝（補間残距離）／（補間速度）

【００８２】続いてステップＳ４３でＣＰＵ１１は次式
に基づいて各関節軸の残距離を求める。 （関節の残距離）＝（関節の終点）−（関節の今回位
置）

【００８３】続いてステップＳ４４でＣＰＵ１１は次式
に基づいて各関節の速度を求める。 （関節の速度）＝（関節の前回の位置）−（関節の今回
位置）

【００８４】続いてステップＳ４５でＣＰＵ１１は次式
に基づいて各関節の加速度を求める。 （関節の加速度）＝（関節の前回の速度）−（関節の今
回速度）

【００８５】続いてステップＳ４６でＣＰＵ１１は各関
節の近傍減速定数を求め、それからデータ作成処理を終
了する。

【００８６】つぎに加速度制御部６１による近傍通過制
御中の加速度の制御について説明する。図５には、本実
施の形態による制御によって変化する加速度の様子が模
式的に示されている。この図に示すように、近傍通過制
御中の関節軸の加速度は、図４のステップＳ４２で求め
た停止時間（近傍減速時間）の間にＳｉｎ波（図６参
照）の−π／２〜π／２［ｒａｄ］の範囲の波形を描い
て、近傍通過制御開始時の関節軸の加速度からなめらか
にゼロに変化する（以後、この制御を加速度制御とす
る）。図６に示すＹ＝ｓｉｎ（Ｘ）の波形においては、
近傍通過制御の開始時および終了時はそれぞれＸ＝−π
／２およびＸ＝＋π／２の相当する。これをＹ＝（ｃｏ
ｓ（Ｘ）＋１）／２に当てはめると、Ｘ＝０からＸ＝π
の範囲に相当する。

【００８７】図７は、図５に示す加速度制御に対する演
算式の導出を説明する説明図であり、同図に従って加速
度制御の演算式の導出過程を説明する。近傍通過制御開
始時の関節軸の速度をＦｎｏｗ、近傍通過制御開始１周
期前の関節軸の速度をＦｐｒｖとすると、近傍通過制御
開始時の加速度Ａｃｃは、つぎの（１）式となる。この
（１）式は、図４のステップＳ４５に相当する。 Ａｃｃ＝Ｆｎｏｗ−Ｆｐｒｖ ・・・（１）

【００８８】ここでＦｎｏｗが小さな値の場合に、つぎ
の（２）式に従って、加速度Ａｃｃの速度に対する影響
を小さくするようにＡｃｃの調整を行い、調整後の加速
度ＫｉｎＡｃｃを求める。 ＫｉｎＡｃｃ＝Ａｃｃ／（１＋｜Ａｃｃ／Ｆｎｏｗ｜） ・・・（２）

【００８９】そして図４のステップＳ４２で求めた停止
時間を補間周期の回数で表したものを減速回数Ｎとし、
近傍通過制御処理中の減速期間に０〜Ｎへカウントアッ
プするカウンタを減速時間カウンタＣとし、これらＮお
よびＣを用いて角度θ１（０〜π）を表すと、つぎの
（３）式のようになる。この（３）式においては、Ｃが
０からＮへ変わるに連れて、θ１は０からπに変化す
る。 θ１＝πＣ／Ｎ ・・・（３）

【００９０】（３）式より得られたθ１を用いて加速度
Ａ１（θ１）を表すと、図５の加速度グラフを表す式と
なる（４）式が得られる。 Ａ１（θ１）＝ＫｉｎＡｃｃ・（ｃｏｓθ１＋１）／２ ・・・（４）

【００９１】そしてこのＡ１（θ１）を０〜θ１まで定
積分することにより速度Ｆ１（θ１）が得られる。速度
Ｆ１（θ１）はつぎの（５）式で表される。 Ｆ１（θ１）＝ＫｉｎＡｃｃ・Ｎ・（Ｓｉｎθ１＋θ１）／（２π） ・・・（５）

【００９２】同様にＦ１（θ１）を０〜θ１まで定積分
することにより移動量Ｓ１（θ１）が得られる。移動量
Ｓ１（θ１）はつぎの（６）式で表される。 Ｓ１（θ１）＝ＫｉｎＡｃｃ・Ｎ² ・（θ１² ／２−ｃｏｓθ１＋１） ／（２π² ） ・・・（６）

【００９３】上記（６）式のうち、定数部分およびＮと
ＫｉｎＡｃｃで決まる部分をつぎの（７）式に示すよう
にβ０とすると、このβ０を用いて（６）式はつぎの
（８）式となる。 β０＝ＫｉｎＡｃｃ・Ｎ² ／２π² ・・・（７） Ｓ１（θ１）＝β０・（θ１² ／２−ｃｏｓθ１＋１） ・・・（８）

【００９４】以上より加速度制御による関節軸の移動量
が上記（７）式および（８）式から求まる。

【００９５】つぎに速度制御部６２による近傍通過制御
中の速度の制御について説明する。図８には、本実施の
形態による制御によって変化する速度の様子が模式的に
示されている。この図に示すように、近傍通過制御中の
関節軸の速度に対して調整を行う対象となるのは、近傍
通過制御開始時の速度から加速度制御により変化する速
度を引いた速度（すなわち、（近傍通過制御開始時の速
度）−（加速度制御で変化する速度））である。この調
整対象速度は、図４のステップＳ４２で求めた停止時間
（近傍減速時間）の間になめらかにゼロに変化する（以
後、この制御を速度制御とする）。

【００９６】図９は、図８に示す速度制御に対する演算
式の導出を説明する説明図であり、同図に従って速度制
御の演算式の導出過程を説明する。速度制御によって調
整される速度をＫｉｎＤｃｃとすると、このＫｉｎＤｃ
ｃは、近傍通過制御開始時の関節軸の速度Ｆｎｏｗ、減
速回数Ｎおよび（２）式の調整後の加速度ＫｉｎＡｃｃ
を用いてつぎの（１０１）式のように表される。 ＫｉｎＤｃｃ＝−（Ｆｎｏｗ＋Ｎ・ＫｉｎＡｃｃ／２） ・・・（１０１）

【００９７】ここで速度制御の加速度の変化を次式のよ
うに表し、Ｘ＝０からＸ＝２πに相当させる。 Ｙ＝（１−ｃｏｓ（Ｘ））／２

【００９８】減速時間カウンタＣおよびＮを用いて角度
θ２（０〜２π）を表すと、つぎの（１０２）式のよう
になる。この（１０２）式においては、Ｃが０からＮへ
変わるに連れて、θ２は０から２πに変化する。 θ２＝２πＣ／Ｎ ・・・（１０２）

【００９９】（１０２）式より得られたθ２を用いて加
速度Ａ２（θ２）を表すと（１０３）式が得られる。 Ａ２（θ２）＝ＫｉｎＤｃｃ・（１−ｃｏｓθ２）／Ｎ ・・・（１０３）

【０１００】そしてこのＡ２（θ２）を０〜θ２まで定
積分することにより速度Ｆ２（θ２）が得られる。速度
Ｆ２（θ２）はつぎの（１０４）式で表される。 Ｆ２（θ２）＝ＫｉｎＡｃｃ・Ｎ・（θ２−Ｓｉｎθ２）／（２π） ・・・（１０４）

【０１０１】同様にＦ２（θ２）を０〜θ２まで定積分
することにより移動量Ｓ２（θ２）が得られる。移動量
Ｓ２（θ２）はつぎの（１０５）式で表される。 Ｓ２（θ２）＝ＫｉｎＤｃｃ・Ｎ・（θ２² ／２−ｃｏｓθ２−１） ／（４π² ） ・・・（１０５）

【０１０２】上記（１０５）式のうち、定数部分および
ＮとＫｉｎＤｃｃで決まる部分をつぎの（１０６）式に
示すようにβ１とすると、このβ１を用いて（１０５）
式はつぎの（１０７）式となる。 β１＝ＫｉｎＤｃｃ・Ｎ／４π² ・・・（１０６） Ｓ２（θ２）＝β１・（θ２² ／２＋ｃｏｓθ２−１） ・・・（１０７）

【０１０３】以上より速度制御による関節軸の移動量が
上記（１０６）式および（１０７）式から求まる。

【０１０４】つぎに残距離制御部６３による近傍通過制
御中の残距離の制御について説明する。図１０には、本
実施の形態による制御によって不足する関節軸の移動量
（以下、不足移動量とする）を調整するためにＳｉｎ波
を用いてなめらかに加速および減速する様子が模式的に
示されており、同図（ａ）および（ｂ）はそれぞれ速度
および加速度の変化を表している。ここで不足移動量
は、近傍通過制御開始時の残距離から加速度制御および
速度制御による合計移動量を引いた距離（すなわち、近
傍通過制御開始時の残距離−加速度制御および速度制御
での合計移動量）である。この不足移動量をＳｉｎ波の
波形を描いて加速したり、減速したりすることによりな
めらかに出力する（以後、この制御を残距離制御とす
る）。

【０１０５】図１１および図１２は、図１０に示す残距
離制御に対する演算式の導出を説明する説明図であり、
同図に従って残距離制御の演算式の導出過程を説明す
る。加速度制御と速度制御との合計減速移動量をＬＥＮ
１とすると、このＬＥＮ１は、加速度制御による移動量
を表すつぎの（２０１）式と、速度制御による移動量を
表すつぎの（２０２）式を用いて、つぎの（２０３）式
で表される。ただしβ０およびβ１はそれぞれ（７）式
および（１０６）式で表される。また、ＮおよびＦｎｏ
ｗはそれぞれ減速回数および近傍通過制御開始時の関節
軸の速度である。 Ｓ１（θ２）＝β０・（２＋π² ／Ｎ） ・・・（２０１） Ｓ２（２π）＝β１・（２π² ）＝ＫｉｎＤｃｃ・Ｎ／２ ・・・（２０２） ＬＥＮ１＝Ｓ１（π）＋Ｓ２（２π）＋Ｆｎｏｗ・Ｎ ・・・（２０３）

【０１０６】また、残距離制御で調整する不足移動量を
ＬＥＮ２とすると、このＬＥＮ２は、残距離をＲＭＮと
してつぎの（２０４）式で表される。 ＬＥＮ２＝ＲＭＮ−ＬＥＮ１ ・・・（２０４）

【０１０７】ここで残距離制御時の加速度の変化は、前
半部分と後半部分とに分割される（図１０参照）。前半
部分は、次式のように表され、Ｘ＝０からＸ＝２πに相
当する。 Ｙ＝（１−ｃｏｓ（Ｘ））／２

【０１０８】一方後半部分は、次式のように表され、Ｘ
＝０からＸ＝２πに相当する。 Ｙ＝−（１−ｃｏｓ（Ｘ））／２

【０１０９】まず上記前半部分について図１１に従って
説明する。減速時間カウンタＣおよびＮを用いて角度θ
３（０〜４π）を表すと、つぎの（２０５）式のように
なる。この（２０５）式においては、Ｃが０からＮへ変
わるに連れて、θ３は０から４πに変化する。 θ３＝４πＣ／Ｎ ・・・（２０５）

【０１１０】（２０５）式より得られたθ３を用いて前
半部分の加速度Ａ３（θ３）を表すとつぎの（２０６）
式が得られる。 Ａ３（θ３）＝４・ＬＥＮ２・（１−ｃｏｓθ３）／Ｎ² ・・・（２０６）

【０１１１】そしてこのＡ３（θ３）を０〜θ３まで定
積分することにより速度Ｆ３（θ３）が得られる。速度
Ｆ３（θ３）はつぎの（２０７）式で表される。 Ｆ３（θ３）＝ＬＥＮ２・（θ３−Ｓｉｎθ３）／（πＮ） ・・・（２０７）

【０１１２】同様にＦ３（θ３）を０〜θ３まで定積分
することにより移動量Ｓ３（θ３）が得られる。移動量
Ｓ３（θ３）はつぎの（２０８）式で表される。 Ｓ３（θ３）＝ＬＥＮ２・（θ３² ／２＋ｃｏｓθ３−１）／（４π² ） ・・・（２０８）

【０１１３】上記（２０８）式のうち、定数部分および
ＬＥＮ２で決まる部分をつぎの（２１０）式に示すよう
にＫｉｎＫｖとすると、このＫｉｎＫｖを用いて（２０
８）式はつぎの（２０９）式となる。 Ｓ３（θ３）＝ＫｉｎＫｖ・（θ３² ／２＋ｃｏｓθ３−１） ・・・（２０９） ＫｉｎＫｖ＝ＬＥＮ２／４π² ・・・（２１０）

【０１１４】続いて上記後半部分について図１２に従っ
て説明する。角度θ３を用いて角度θ４（０〜２π）を
表すと、つぎの（２１１）式のようになる。この（２１
１）式においては、ＣがＮ／２からＮへ変わるに連れ
て、θ４は０から２πに変化する。 θ４＝θ３−２π ・・・（２１１）

【０１１５】このθ４を用いて後半部分の加速度Ａ４
（θ４）を表すとつぎの（２１２）式が得られる。 Ａ４（θ４）＝４・ＬＥＮ２・（１−ｃｏｓθ４）／Ｎ² ・・・（２１２）

【０１１６】そしてこのＡ４（θ４）を０〜θ４まで定
積分することにより速度Ｆ４（θ４）が得られる。速度
Ｆ４（θ４）はつぎの（２１３）式で表される。 Ｆ４（θ４）＝２・ＬＥＮ２／Ｎ−ＬＥＮ２・（θ４−Ｓｉｎθ４） ／（πＮ） ・・・（２１３）

【０１１７】同様にＦ４（θ４）を０〜θ４まで定積分
することにより移動量Ｓ４（θ４）が得られる。移動量
Ｓ４（θ４）はつぎの（２１４）式で表される。 Ｓ４（θ４）＝ＬＥＮ２・（−θ４² ／２−ｃｏｓθ４＋１＋２π・θ４） ／（４π² ）＋ＬＥＮ２／２ ・・・（２１４）

【０１１８】上記（２１４）式のうち、定数部分および
ＬＥＮ２で決まる部分を（２１０）式のＫｉｎＫｖで表
すとつぎの（２１５）式となる。 Ｓ４（θ４）＝ＫｉｎＫｖ・（−θ４² ／２−ｃｏｓθ４＋１＋２π・θ４） ＋ＬＥＮ２／２ ・・・（２１５）

【０１１９】以上より残距離制御による関節軸の移動量
が上記（２０９）式、（２１０）式および（２１５）式
から求まる。

【０１２０】この実施の形態においては、図４のステッ
プＳ４６で求めた関節の近傍減速定数は、上記（７）
式、（１０６）式および（２１０）式で表されるβ０、
β１およびＫｉｎＫＶである。

【０１２１】図１３は、加速度制御、速度制御および残
距離制御の合計移動量の演算式の導出を説明する説明図
である。合計移動量Ｓａｌｌ（Ｃ）は、減速時間カウン
タＣが０〜Ｎ／２の場合にはつぎの（３０１）式で求め
られる。 Ｓａｌｌ（Ｃ）＝Ｓ１（θ１）＋Ｓ２（θ２）＋Ｓ３（θ３） ・・・（３０１）

【０１２２】一方減速時間カウンタＣがＮ／２〜Ｎの場
合には、合計移動量Ｓａｌｌ（Ｃ）はつぎの（３０２）
式で求められる。 Ｓａｌｌ（Ｃ）＝Ｓ１（θ１）＋Ｓ２（θ２）＋Ｓ４（θ４） ・・・（３０２）

【０１２３】以上の（３０１）式および（３０２）式に
基づいて、近傍通過制御時の各関節軸の最適な指令値を
演算することができる。

【０１２４】図１４は、上記演算に基づいて近傍通過制
御処理中の減速時の速度（同図において菱形のプロッ
ト）および加速度（同図において四角のプロット）を求
めた結果を示すグラフであり、同図には比較のため従来
の演算方法による速度（同図において三角のプロット）
および加速度（同図において×印のプロット）のグラフ
も記載されている。なおこの検証においては、近傍通過
制御開始時の加速度Ａｃｃはゼロ、近傍通過制御開始時
の関節軸の速度Ｆｎｏｗは−３００００、残距離ＲＭＮ
は−３０００００、減速回数Ｎは２０である。図１４よ
り、近傍通過制御処理の開始時、近傍通過制御処理の途
中および近傍通過制御処理の終了時において速度および
加速度がともになめらかに変化しているのが分かる。

【０１２５】上記実施の形態によれば、現在の移動区間
における移動方向の成分の移動が停止するのに要する停
止時間内に想定される移動距離のうち、つぎの移動区間
の進行方向の成分の距離が所定の近傍量よりも小さい場
合に、近傍通過制御処理が開始されるため、近傍通過制
御処理の終了時にロボットの基準点が近傍範囲外に出な
いように自動的に制御されるので、近傍範囲内で近傍通
過制御を終了させるために加速および減速時間を移動プ
ログラムごとに調整する手間が削減され、近傍通過制御
処理に伴う移動プログラムを微調整する手間を軽減する
ことができる。

【０１２６】また、上記実施の形態によれば、近傍通過
制御開始時の現在の移動区間における移動方向の成分の
移動が停止するのに要する停止時間、近傍通過制御開始
時の現在の移動区間における各関節の現在位置から終点
までの残距離、近傍通過制御開始時の各関節の速度、近
傍通過制御開始時の各関節の加速度、およびそれら停止
時間と移動残距離と速度と加速度に基づいて決まる定数
が求まるため、その定数を用いて、近傍通過制御処理の
実行中の関節の加速度がなめらかにゼロになるような第
１の位置制御値、近傍通過制御処理中の関節の速度がな
めらかにゼロになるような第２の位置制御値、および近
傍通過制御処理の実行中の関節の移動量の不足分がなめ
らかに変化するような第３の位置制御値を求めることが
でき、さらにそれら第１の位置制御値、第２の位置制御
値および第３の位置制御値を合計することにより近傍通
過制御処理中の関節の位置を求めることができるため、
近傍通過制御の開始時、途中および終了時の関節の加速
度および速度をなめらかに変化させるような制御指令値
が得られる。従って近傍通過制御の開始時と、残距離補
正が反転する時と、近傍通過制御の終了時において関節
の加速度および速度がゆるやかに変化するので、近傍通
過制御処理時における加速度の乱れが殆どなく、振動等
が発生せずにロボットがなめらかに動く。

【０１２７】なお本発明は、上記実施の形態の説明およ
び図示例に限らず、種々変更可能である。例えば上記実
施の形態では加速度制御、速度制御および残距離制御に
よる関節軸の移動量を三角関数を用いて表したが、これ
に限らず、近傍通過制御処理の開始時、途中および終了
時の関節軸の速度および加速度がなめらかに変化するよ
うな関数であれば、如何なる関数を用いてもよい。

【０１２８】

【発明の効果】本発明に係るロボット制御装置によれ
ば、現在の移動区間における移動方向の成分の移動が停
止するのに要する停止時間内に想定される移動距離のう
ち、つぎの移動区間の進行方向の成分の距離が所定の近
傍量よりも小さい場合に、近傍通過制御処理が開始され
るため、近傍通過制御処理の終了時にロボットの基準点
が近傍範囲外に出ないように自動的に制御される。従っ
てロボットの基準点が近傍範囲内にあるうちに近傍通過
制御処理が終了するので、近傍範囲内で近傍通過制御を
終了させるために加速および減速時間を移動プログラム
ごとに調整する手間が削減され、近傍通過制御処理に伴
う移動プログラムを微調整する手間を軽減することがで
きる。

【０１２９】また、本発明に係るロボット制御装置によ
れば、近傍通過制御開始時の現在の移動区間における移
動方向の成分の移動が停止するのに要する停止時間、近
傍通過制御開始時の現在の移動区間における各関節の現
在位置から終点までの残距離、近傍通過制御開始時の各
関節の速度、近傍通過制御開始時の各関節の加速度、お
よびそれら停止時間と移動残距離と速度と加速度に基づ
いて決まる定数が求まるため、その定数を用いて近傍通
過制御の開始時、途中および終了時の関節の加速度およ
び速度をなめらかに変化させるように制御指令値を生成
することができる。

【０１３０】また、本発明に係るロボット制御装置によ
れば、近傍通過制御処理の実行中の関節の加速度がなめ
らかにゼロになるような第１の位置制御値、近傍通過制
御処理中の関節の速度がなめらかにゼロになるような第
２の位置制御値、および近傍通過制御処理の実行中の関
節の移動量の不足分がなめらかに変化するような第３の
位置制御値が求まり、それら第１の位置制御値、第２の
位置制御値および第３の位置制御値を合計することによ
り近傍通過制御処理中の関節の位置が求まるため、近傍
通過制御の開始時、途中および終了時の関節の加速度お
よび速度をなめらかに変化させるような制御指令値が得
られる。従って近傍通過制御の開始時と、残距離補正が
反転する時と、近傍通過制御の終了時において関節の加
速度および速度がゆるやかに変化するので、近傍通過制
御処理時における加速度の乱れが殆どなく、振動等が発
生せずにロボットがなめらかに動く。

【０１３１】また、本発明に係るロボット制御装置によ
れば、第１の位置制御値、第２の位置制御値および第３
の位置制御値は、何れも三角関数で表されるため、近傍
通過制御の開始時、途中および終了時の関節の加速度お
よび速度をなめらかに変化させるような制御指令値が得
られる。従って近傍通過制御の開始時と、残距離補正が
反転する時と、近傍通過制御の終了時において関節の加
速度および速度がゆるやかに変化するので、近傍通過制
御処理時における加速度の乱れが殆どなく、振動等が発
生せずにロボットがなめらかに動く。

【０１３２】また、本発明に係るロボット制御装置によ
れば、現在の移動区間における移動方向の成分の移動が
停止するのに要する停止時間内に想定される移動距離の
うち、つぎの移動区間の進行方向の成分の距離が所定の
近傍量よりも小さい場合に、近傍通過制御処理が開始さ
れるため、近傍通過制御処理の終了時にロボットの基準
点が近傍範囲外に出ないように自動的に制御されるの
で、近傍範囲内で近傍通過制御を終了させるために加速
および減速時間を移動プログラムごとに調整する手間が
削減され、近傍通過制御処理に伴う移動プログラムを微
調整する手間を軽減することができる。加えて近傍通過
制御開始時の現在の移動区間における移動方向の成分の
移動が停止するのに要する停止時間、近傍通過制御開始
時の現在の移動区間における各関節の現在位置から終点
までの残距離、近傍通過制御開始時の各関節の速度、近
傍通過制御開始時の各関節の加速度、およびそれら停止
時間と移動残距離と速度と加速度に基づいて決まる定数
が求まるため、その定数を用いて近傍通過制御の開始
時、途中および終了時の関節の加速度および速度をなめ
らかに変化させるように制御指令値を生成することがで
きる。

【０１３３】本発明に係るロボット制御方法によれば、
現在の移動区間における移動方向の成分の移動が停止す
るのに要する停止時間内に想定される移動距離のうち、
つぎの移動区間の進行方向の成分の距離が所定の近傍量
よりも小さい場合に、近傍通過制御処理が開始されるた
め、近傍通過制御処理の終了時にロボットの基準点が近
傍範囲外に出ないように自動的に制御される。従ってロ
ボットの基準点が近傍範囲内にあるうちに近傍通過制御
処理が終了するので、近傍範囲内で近傍通過制御を終了
させるために加速および減速時間を移動プログラムごと
に調整する手間が削減され、近傍通過制御処理に伴う移
動プログラムを微調整する手間を軽減することができ
る。

【０１３４】また、本発明に係るロボット制御方法によ
れば、近傍通過制御開始時の現在の移動区間における移
動方向の成分の移動が停止するのに要する停止時間、近
傍通過制御開始時の現在の移動区間における各関節の現
在位置から終点までの残距離、近傍通過制御開始時の各
関節の速度、近傍通過制御開始時の各関節の加速度、お
よびそれら停止時間と移動残距離と速度と加速度に基づ
いて決まる定数が求まるため、その定数を用いて近傍通
過制御の開始時、途中および終了時の関節の加速度およ
び速度をなめらかに変化させるように制御指令値を生成
することができる。

【０１３５】また、本発明に係るロボット制御方法によ
れば、近傍通過制御処理の実行中の関節の加速度がなめ
らかにゼロになるような第１の位置制御値、近傍通過制
御処理中の関節の速度がなめらかにゼロになるような第
２の位置制御値、および近傍通過制御処理の実行中の関
節の移動量の不足分がなめらかに変化するような第３の
位置制御値が求まり、それら第１の位置制御値、第２の
位置制御値および第３の位置制御値を合計することによ
り近傍通過制御処理中の関節の位置が求まるため、近傍
通過制御の開始時、途中および終了時の関節の加速度お
よび速度をなめらかに変化させるような制御指令値が得
られる。従って近傍通過制御の開始時と、残距離補正が
反転する時と、近傍通過制御の終了時において関節の加
速度および速度がゆるやかに変化するので、近傍通過制
御処理時における加速度の乱れが殆どなく、振動等が発
生せずにロボットがなめらかに動く。

【０１３６】また、本発明に係るロボット制御方法によ
れば、第１の位置制御値、第２の位置制御値および第３
の位置制御値は、何れも三角関数で表されるため、近傍
通過制御の開始時、途中および終了時の関節の加速度お
よび速度をなめらかに変化させるような制御指令値が得
られる。従って近傍通過制御の開始時と、残距離補正が
反転する時と、近傍通過制御の終了時において関節の加
速度および速度がゆるやかに変化するので、近傍通過制
御処理時における加速度の乱れが殆どなく、振動等が発
生せずにロボットがなめらかに動く。

【０１３７】また、本発明に係るロボット制御方法によ
れば、現在の移動区間における移動方向の成分の移動が
停止するのに要する停止時間内に想定される移動距離の
うち、つぎの移動区間の進行方向の成分の距離が所定の
近傍量よりも小さい場合に、近傍通過制御処理が開始さ
れるため、近傍通過制御処理の終了時にロボットの基準
点が近傍範囲外に出ないように自動的に制御されるの
で、近傍範囲内で近傍通過制御を終了させるために加速
および減速時間を移動プログラムごとに調整する手間が
削減され、近傍通過制御処理に伴う移動プログラムを微
調整する手間を軽減することができる。加えて近傍通過
制御開始時の現在の移動区間における移動方向の成分の
移動が停止するのに要する停止時間、近傍通過制御開始
時の現在の移動区間における各関節の現在位置から終点
までの残距離、近傍通過制御開始時の各関節の速度、近
傍通過制御開始時の各関節の加速度、およびそれら停止
時間と移動残距離と速度と加速度に基づいて決まる定数
が求まるため、その定数を用いて近傍通過制御の開始
時、途中および終了時の関節の加速度および速度をなめ
らかに変化させるように制御指令値を生成することがで
きる。

【図面の簡単な説明】

【図１】 本発明に係る多関節ロボット駆動制御装置の
機能構成を示すブロック図である。

【図２】 本発明に係るロボット制御方法において近傍
通過制御を開始するか否かを判断する処理の一例を示す
フローチャートである。

【図３】 本発明に係るロボット制御方法において近傍
通過制御開始タイミングを調整した場合のポイントＰ３
近傍におけるロボットの運転経路を示す経路図である。

【図４】 図２のステップＳ３６における近傍通過制御
開始時のデータ作成処理の一例の詳細な手順を説明する
フローチャートである。

【図５】 本実施の形態による制御によって変化する加
速度の様子を示す模式図である。

【図６】 本実施の形態による制御に用いられたＳｉｎ
波を示す波形図である。

【図７】 本実施の形態による加速度制御の演算式の導
出を説明する説明図である。

【図８】 本実施の形態による制御によって変化する速
度の様子を示す模式図である。

【図９】 本実施の形態による速度制御の演算式の導出
を説明する説明図である。

【図１０】 本実施の形態による制御によって不足する
関節軸の移動量を調整する際の速度および加速度の様子
を示す模式図である。

【図１１】 本実施の形態による残距離制御の演算式の
導出を説明する説明図である。

【図１２】 本実施の形態による残距離制御の演算式の
導出を説明する説明図である。

【図１３】 本実施の形態による加速度制御、速度制御
および残距離制御の合計移動量の演算式の導出を説明す
る説明図である。

【図１４】 本実施の形態により近傍通過制御処理中の
減速時の速度および加速度を求めた結果を示すグラフで
ある。

【図１５】 一般的な水平多関節ロボットの一例を示す
模式図である。

【図１６】 各通過ポイントを通過する場合のロボット
の運転経路の一例を示す経路図である。

【図１７】 図１６のポイントＰ１におけるロボットの
姿勢を示す模式図である。

【図１８】 図１６のポイントＰ２におけるロボットの
姿勢を示す模式図である。

【図１９】 図１６のポイントＰ３におけるロボットの
姿勢を示す模式図である。

【図２０】 図１６のポイントＰ４におけるロボットの
姿勢を示す模式図である。

【図２１】 図１６の各移動区間におけるハンドのＸ軸
方向およびＹ軸方向の速度と加速度の変化の様子を示す
グラフである。

【図２２】 一般的な近傍通過制御によるロボットの運
転経路を示す経路図である。

【図２３】 多関節ロボットの一般的な駆動制御装置の
構成を示すブロック図である。

【図２４】 多関節ロボットの従来の駆動制御装置の機
能構成を示すブロック図である。

【図２５】 従来の近傍通過制御を開始するか否かを判
断する処理のフローチャートである。

【図２６】 近傍通過制御時のポイントＰ３近傍におけ
るロボットの好ましい運転経路を示す経路図である。

【図２７】 近傍通過制御時のポイントＰ３近傍におけ
るロボットの好ましくない運転経路を示す経路図であ
る。

【図２８】 図２５のステップＳ１３における近傍通過
制御開始時のデータ作成処理の詳細な手順を説明するフ
ローチャートである。

【図２９】 従来の近傍通過制御による関節軸の速度の
変化を示すグラフである。

【図３０】 従来の近傍通過制御により図２２に示すポ
イントＰ２を通過した場合の、ハンド先端のＸ軸方向お
よびＹ軸方向の速度と加速度の変化を示すグラフであ
る。

【符号の説明】

１ 制御装置、２ 駆動装置、１１ ＣＰＵ、１２ プ
ログラム・位置決めポイント保存用メモリ、１３ 演算
用ワークメモリ、１４ Ｏ／Ｓメモリ、１５入出力イン
ターフェース、１６ サーボアンプインターフェース、
２１，２２，２３ サーボアンプ、２６，２７，２８
モータ、３１ 残距離演算部、３２残距離判定部、３３
フラグ制御部、４１ 関節終点記憶実行部、４２ 停
止時間演算部、４３ 関節残距離演算部、４４ 関節速
度演算部、４５ 速度演算部、４６ 関節現在位置演算
部、５１ 次区間進行距離演算部、５２ 次区間進行距
離判定部、５３ 関節加速度演算部、５４ 関節近傍減
速定数演算部、６１加速度制御部、６２ 速度制御部、
６３ 残距離制御部、６４ 加算部。

───────────────────────────────────────────────────── フロントページの続き (56)参考文献 特開 平８−123531（ＪＰ，Ａ) 特開 平７−200034（ＪＰ，Ａ) 特開 昭64−26911（ＪＰ，Ａ) 特許2761795（ＪＰ，Ｂ２) (58)調査した分野(Int.Cl.⁷，ＤＢ名) B25J 9/10 - 9/22 B25J 13/00 - 13/08 G05D 3/00

Claims (8)

(57)【特許請求の範囲】
1. 【請求項１】 ロボットの基準点が通過するように予め
   指定された通過点に対して、該通過点の直前の移動区間
   における減速と該通過点の直後の移動区間における加速
   とを同時に行うことによって、該通過点を通らずにその
   通過点の近傍を通過するようにロボットの動作を制御す
   る近傍通過制御処理を行う制御装置であって、 通過点の通過前に前記基準点の現在の移動区間における
   残距離を求める残距離演算部と、 該残距離演算部により求められた残距離を所定の近傍量
   と比較する残距離判定部と、 前記残距離が前記近傍量よりも小さい場合にのみ、前記
   基準点の、現在の移動区間における移動方向の成分の移
   動が停止するのに要する停止時間を求める停止時間演算
   部と、 該停止時間演算部により求められた停止時間内に前記基
   準点が移動すると想定される距離のうち、つぎの移動区
   間の進行方向の成分の距離を演算して求める次区間進行
   距離演算部と、 該次区間進行距離演算部により求められた進行方向成分
   の距離を所定の近傍量と比較し、前記進行方向成分の距
   離が前記近傍量よりも小さい場合にのみ、前記近傍通過
   制御処理を開始する次区間進行距離判定部と、 を具備することを特徴とするロボットの制御装置。
2. 【請求項２】 少なくとも１つの移動自在な関節を有す
   るロボットの基準点が通過するように予め指定された通
   過点に対して、該通過点の直前の移動区間における減速
   と該通過点の直後の移動区間における加速とを同時に行
   うことによって、該通過点を通らずにその通過点の近傍
   を通過するようにロボットの動作を制御する近傍通過制
   御処理を行う制御装置であって、前記 近傍通過制御処理開始時の前記基準点の、現在の移
   動区間における移動方向の成分の移動が停止するのに要
   する停止時間を求める停止時間演算部と、前記 近傍通過制御処理時に現在の移動区間における前記
   各関節の現在位置から終点までの残距離を求める関節残
   距離演算部と、前記 近傍通過制御処理時に前記各関節の速度を求める関
   節速度演算部と、前記 近傍通過制御処理時に前記各関節の加速度を求める
   関節加速度演算部と、 前記 近傍通過制御処理開始時の停止時間、該近傍通過制
   御処理開始時の関節の移動残距離、該近傍通過制御処理
   開始時の関節の速度、および該近傍通過制御処理開始時
   の関節の加速度に基づいて決まる近傍減速定数を算出す
   る関節近傍減速定数演算部と、 前記 近傍通過制御処理の実行中の関節の加速度が前記停
   止時間中になめらかにゼロになるような第１の位置制御
   値を、前記近傍減速定数を用いて演算して求める加速度
   制御部と、 前記第１の位置制御値に基づいて前記近傍通過制御処理
   の実行中に変化する関節の速度を考慮して、該近傍通過
   制御処理中の関節の速度が前記停止時間中になめらかに
   ゼロになるような第２の位置制御値を、前記近傍減速定
   数を用いて演算して求める速度制御部と、 前記第１の位置制御値および前記第２の位置制御値に基
   づいて不足する関節の移動量がなめらかに変化するよう
   な第３の位置制御値を、前記近傍減速定数を用いて演算
   して求める残距離制御部と、 前記第１の位置制御値、前記第２の位置制御値および前
   記第３の位置制御値を合計して前記近傍通過制御処理中
   の関節の位置を求める加算部と、 を具備することを特徴とするロボットの制御装置。
3. 【請求項３】 前記第１の位置制御値、前記第２の位置
   制御値および前記第３の位置制御値は、何れも三角関数
   で表されることを特徴とする請求項２に記載のロボット
   の制御装置。
4. 【請求項４】 少なくとも１つの移動自在な関節を有す
   るロボットの基準点が通過するように予め指定された通
   過点に対して、該通過点の直前の移動区間における減速
   と該通過点の直後の移動区間における加速とを同時に行
   うことによって、該通過点を通らずにその通過点の近傍
   を通過するようにロボットの動作を制御する近傍通過制
   御処理を行う制御装置であって、 通過点の通過前に前記基準点の現在の移動区間における
   残距離を求める残距離演算部と、 該残距離演算部により求められた残距離を所定の近傍量
   と比較する残距離判定部と、 前記残距離が前記近傍量よりも小さい場合または近傍通
   過制御開始時に、前記基準点の、現在の移動区間におけ
   る移動方向の成分の移動が停止するのに要する停止時間
   を求める停止時間演算部と、前記残距離が前記近傍量よりも小さい場合における 該停
   止時間演算部により求められた停止時間内に前記基準点
   が移動すると想定される距離のうち、つぎの移動区間の
   進行方向の成分の距離を演算して求める次区間進行距離
   演算部と、 該次区間進行距離演算部により求められた進行方向成分
   の距離を所定の近傍量と比較し、前記進行方向成分の距
   離が前記近傍量よりも小さい場合にのみ、前記近傍通過
   制御処理を開始する次区間進行距離判定部と、前記 近傍通過制御処理時に現在の移動区間における前記
   各関節の現在位置から終点までの残距離を求める関節残
   距離演算部と、前記 近傍通過制御処理時に前記各関節の速度を求める関
   節速度演算部と、前記 近傍通過制御処理時に前記各関節の加速度を求める
   関節加速度演算部と、 前記 近傍通過制御処理開始時の停止時間、該近傍通過制
   御処理開始時の関節の移動残距離、該近傍通過制御処理
   開始時の関節の速度、および該近傍通過制御処理開始時
   の関節の加速度に基づいて決まる近傍減速定数を算出す
   る関節近傍減速定数演算部と、 前記近傍通過制御処理の実行中の関節の加速度が前記近
   傍通過制御処理開始時の停止時間中になめらかにゼロに
   なるような第１の位置制御値を、前記近傍減速定数を用
   いて演算して求める加速度制御部と、 前記第１の位置制御値に基づいて前記近傍通過制御処理
   の実行中に変化する関節の速度を考慮して、該近傍通過
   制御処理中の関節の速度が前記近傍通過制御処理開始時
   の停止時間中になめらかにゼロになるような第２の位置
   制御値を、前記近傍減速定数を用いて演算して求める速
   度制御部と、 前記第１の位置制御値および前記第２の位置制御値に基
   づいて不足する関節の移動量がなめらかに変化するよう
   な第３の位置制御値を、前記近傍減速定数を用いて演算
   して求める残距離制御部と、 前記第１の位置制御値、前記第２の位置制御値および前
   記第３の位置制御値を合計して前記近傍通過制御処理中
   の関節の位置を求める加算部と、を具備する ことを特徴
   とするロボットの制御装置。
5. 【請求項５】 ロボットの基準点が通過するように予め
   指定された通過点に対して、該通過点の直前の移動区間
   における減速と該通過点の直後の移動区間における加速
   とを同時に行うことによって、該通過点を通らずにその
   通過点の近傍を通過するようにロボットの動作を制御す
   る近傍通過制御処理を行う制御方法において、 通過点の通過前に前記基準点の現在の移動区間における
   残距離を求める残距離演算部と、 該残距離演算部により求められた残距離を所定の近傍量
   と比較する工程と、 前記残距離が前記近傍量よりも小さい場合にのみ、前記
   基準点の、現在の移動区間における移動方向の成分の移
   動が停止するのに要する停止時間を求める工程と、 該停止時間演算部により求められた停止時間内に前記基
   準点が移動すると想定される距離のうち、つぎの移動区
   間の進行方向の成分の距離を演算して求める工程と、 該次区間進行距離演算部により求められた進行方向成分
   の距離を所定の近傍量と比較し、前記進行方向成分の距
   離が前記近傍量よりも小さい場合にのみ、前記近傍通過
   制御処理を開始する工程と、 を含むことを特徴とするロボットの制御方法。
6. 【請求項６】 少なくとも１つの移動自在な関節を有す
   るロボットの基準点が通過するように予め指定された通
   過点に対して、該通過点の直前の移動区間における減速
   と該通過点の直後の移動区間における加速とを同時に行
   うことによって、該通過点を通らずにその通過点の近傍
   を通過するようにロボットの動作を制御する近傍通過制
   御処理を行う制御方法において、前記 近傍通過制御処理開始時の前記基準点の、現在の移
   動区間における移動方向の成分の移動が停止するのに要
   する停止時間を求める工程と、前記 近傍通過制御処理時に現在の移動区間における前記
   各関節の現在位置から終点までの残距離を求める工程
   と、前記 近傍通過制御処理時に前記各関節の速度を求める工
   程と、前記 近傍通過制御処理時に前記各関節の加速度を求める
   工程と、前記 近傍通過制御処理開始時の停止時間、該近傍通過制
   御処理開始時の関節の移動残距離、該近傍通過制御処理
   開始時の関節の速度、および該近傍通過制御処理開始時
   の関節の加速度に基づいて決まる近傍減速定数を算出す
   る工程と、前記 近傍通過制御処理の実行中の関節の加速度が前記停
   止時間中になめらかにゼロになるような第１の位置制御
   値を、前記近傍減速定数を用いて演算して求める工程
   と、 前記第１の位置制御値に基づいて前記近傍通過制御処理
   の実行中に変化する関節の速度を考慮して、該近傍通過
   制御処理中の関節の速度が前記停止時間中になめらかに
   ゼロになるような第２の位置制御値を、前記近傍減速定
   数を用いて演算して求める工程と、 前記第１の位置制御値および前記第２の位置制御値に基
   づいて不足する関節の移動量がなめらかに変化するよう
   な第３の位置制御値を、前記近傍減速定数を用いて演算
   して求める工程と、 前記第１の位置制御値、前記第２の位置制御値および前
   記第３の位置制御値を合計して前記近傍通過制御処理中
   の関節の位置を求める工程と、 を含むことを特徴とするロボットの制御方法。
7. 【請求項７】 前記第１の位置制御値、前記第２の位置
   制御値および前記第３の位置制御値は、何れも三角関数
   で表されることを特徴とする請求項６に記載のロボット
   の制御方法。
8. 【請求項８】 少なくとも１つの移動自在な関節を有す
   るロボットの基準点が通過するように予め指定された通
   過点に対して、該通過点の直前の移動区間における減速
   と該通過点の直後の移動区間における加速とを同時に行
   うことによって、該通過点を通らずにその通過点の近傍
   を通過するようにロボットの動作を制御する近傍通過制
   御処理を行う制御方法において、 通過点の通過前に前記基準点の現在の移動区間における
   残距離を求める工程と、 該残距離演算部により求められた残距離を所定の近傍量
   と比較する工程と、 前記残距離が前記近傍量よりも小さい場合または近傍通
   過制御開始時に、前記基準点の、現在の移動区間におけ
   る移動方向の成分の移動が停止するのに要する停止時間
   を求める工程と、前記残距離が前記近傍量よりも小さい場合における 該停
   止時間演算部により求められた停止時間内に前記基準点
   が移動すると想定される距離のうち、つぎの移動区間の
   進行方向の成分の距離を演算して求める工程と、 該次区間進行距離演算部により求められた進行方向成分
   の距離を所定の近傍量と比較し、前記進行方向成分の距
   離が前記近傍量よりも小さい場合にのみ、前記近傍通過
   制御処理を開始する工程と、前記 近傍通過制御処理時に現在の移動区間における前記
   各関節の現在位置から終点までの残距離を求める工程
   と、前記 近傍通過制御処理時に前記各関節の速度を求める工
   程と、前記 近傍通過制御処理時に前記各関節の加速度を求める
   工程と、前記 近傍通過制御処理開始時の停止時間、該近傍通過制
   御処理開始時の関節の移動残距離、該近傍通過制御処理
   開始時の関節の速度、および該近傍通過制御処理開始時
   の関節の加速度に基づいて決まる近傍減速定数を算出す
   る工程と、前記近傍通過制御処理の実行中の関節の加速度が前記近
   傍通過制御処理開始時の停止時 間中になめらかにゼロに
   なるような第１の位置制御値を、前記近傍減速定数を用
   いて演算して求める工程と、 前記第１の位置制御値に基づいて前記近傍通過制御処理
   の実行中に変化する関節の速度を考慮して、該近傍通過
   制御処理中の関節の速度が前記近傍通過制御処理開始時
   の停止時間中になめらかにゼロになるような第２の位置
   制御値を、前記近傍減速定数を用いて演算して求める工
   程と、 前記第１の位置制御値および前記第２の位置制御値に基
   づいて不足する関節の移動量がなめらかに変化するよう
   な第３の位置制御値を、前記近傍減速定数を用いて演算
   して求める工程と、 前記第１の位置制御値、前記第２の位置制御値および前
   記第３の位置制御値を合計して前記近傍通過制御処理中
   の関節の位置を求める工程と、 を含むことを特徴とするロボットの制御装置。