JP6476635B2 - ロボットの制御装置及び制御方法 - Google Patents

Description

本発明は、ロボットの制御装置及び制御方法に関する。

従来、ロボットのアームを駆動するサーボモータの速度パターンを、最大速度条件及び最大加減速度条件に基づいて最適化する方法がある（非特許文献１参照）。非特許文献１に記載の方法によれば、アームの各位置において最大の速度となるように速度パターンが作成されるため、アームの動作時間を短縮することができる。

The 2nd IFToMM Asian Conference on Mechanism and Machine Science November 7-10, 2012 "Planning manipulator trajectories under dynamics using minimum-time shortcuts" by Quang-Cuong Pham

ところで、非特許文献１に記載の方法では、最適の速度パターンを作成するための計算負荷が大きいため、自動運転時にリアルタイムで速度パターンを作成することができない。このため、非特許文献１に記載の方法は、自動運転に先立ちオフラインで予め作成した速度パターンを、ロボットの自動運転時の動作で再生するというように利用されている。

しかしながら、ベルトコンベア上を流れるワークの位置を特定してアームで掴むような動作では、その都度アームの動作が異なるため、オフラインで予め作成した速度パターンを用いることはできない。若しくは、膨大な量の速度パターンを事前に作成する必要があるため、現実的ではない。

本発明は、こうした実情に鑑みてなされたものであり、サーボモータの速度パターンを簡易な処理で最適化することにより、最適の速度パターンを自動運転時にリアルタイムで作成可能とすることを主たる目的とするものである。

第１の手段は、サーボモータにより駆動されるアームを備えるロボットの制御装置であって、前記サーボモータの動作開始位置から動作終了位置まで各位置における前記サーボモータの上限速度を算出する上限速度算出手段と、前記動作開始位置側から前記動作終了位置側へ各位置における速度にその位置における最大加速度を加算した第１速度を、前記第１速度に対応する前記上限速度を上限として算出する第１速度算出手段と、前記動作終了位置側から前記動作開始位置側へ各位置における速度にその位置における最大減速度を加算した第２速度を、前記第２速度に対応する前記上限速度を上限として算出する第２速度算出手段と、前記動作開始位置から前記動作終了位置までの各位置において、前記第１速度算出手段により算出された前記第１速度及び前記第２速度算出手段により算出された前記第２速度のうち低い方の速度を選択して前記サーボモータの速度パターンを作成する速度パターン作成手段と、を備えることを特徴とする。

上記構成によれば、サーボモータの動作開始位置から動作終了位置まで各位置におけるサーボモータの上限速度が算出される。例えば、この上限速度は、サーボモータの仕様によるサーボモータの最大速度や、駆動されるアームの最大速度や最大加速度、コリオリ力によるトルク条件等に基づいて算出される。

そして、第１速度算出手段により、動作開始位置側から動作終了位置側へ各位置における速度にその位置における最大加速度を加算した第１速度が算出される。ただし、第１速度に対応する上限速度が上限とされるため、第１速度が上限速度を超える場合には、第１速度は上限速度に制限される。このため、第１速度は、上限速度を超えない範囲で最大の速度として算出され、最大速度条件を満たすことができる。

また、第２速度算出手段により、動作終了位置側から動作開始位置側へ各位置における速度にその位置における最大減速度を加算した第２速度が算出される。なお、最大減速度は、動作開始位置側から動作終了位置側へ移動する時の最大減速度であり、動作終了位置側から動作開始位置側へ移動する時においては第２速度の最大上昇量となる。ただし、第２速度に対応する上限速度が上限とされるため、第２速度が上限速度を超える場合には、第２速度は上限速度に制限される。このため、第２速度は、上限速度を超えない範囲で、最大の速度として算出され、最大速度条件を満たすことができる。

ここで、共通の位置において、例えば第１速度よりも第２速度が低い場合は、第１速度はその位置での最大減速度条件を満たさないこととなる。この点、動作開始位置から動作終了位置までの各位置において、第１速度及び第２速度のうち低い方の速度が選択されてサーボモータの速度パターンが作成される。このため、この速度パターンは、最大加減速度条件を満たすことができる。したがって、速度パターンを簡易な処理で最適化することができ、最適の速度パターンをリアルタイムで作成することが可能となる。

第２の手段では、前記第１速度算出手段は、前記動作開始位置から前記動作終了位置まで各位置における速度にその位置における最大加速度を加算した第１速度を、前記第１速度に対応する前記上限速度を上限として順次算出し、前記第２速度算出手段は、前記動作終了位置から前記動作開始位置まで各位置における速度にその位置における最大減速度を加算した第２速度を、前記第２速度に対応する前記第１速度を上限として順次算出し、前記速度パターン作成手段は、前記動作開始位置から前記動作終了位置までの各位置において、前記第２速度算出手段により算出された前記第２速度を用いて前記速度パターンを作成する。

上記構成によれば、第１速度算出手段により、動作開始位置から動作終了位置まで各位置における速度にその位置における最大加速度を加算した第１速度が、第１速度に対応する上限速度を上限として順次算出される。すなわち、動作開始位置から動作終了位置までの第１速度パターンが作成される。この第１速度パターンは、上限速度を超えない範囲で最大の速度として算出され、最大速度条件を満たすことができる。

また、第２速度算出手段により、動作終了位置から動作開始位置まで各位置における速度にその位置における最大減速度を加算した第２速度が、第２速度に対応する第１速度を上限として順次算出される。すなわち、動作終了位置から動作開始位置までの第２速度パターンが作成される。この第２速度パターンは、上限速度を超えない範囲で最大の速度として算出された第１速度を上限としており、最大速度条件を満たすことができる。

そして、速度パターン作成手段により、動作開始位置から動作終了位置までの各位置において、第２速度算出手段により算出された第２速度を用いて速度パターンが作成される。したがって、共通の位置において、第１速度よりも第２速度が低い場合は、第２速度が選択されて速度パターンが作成されることとなる。このため、この速度パターンは、最大加減速度条件を満たすことができる。

第３の手段では、前記第２速度算出手段は、前記動作終了位置から前記動作開始位置まで各位置における速度にその位置における最大減速度を加算した第２速度を、前記第２速度に対応する前記上限速度を上限として順次算出し、前記第１速度算出手段は、前記動作開始位置から前記動作終了位置まで各位置における速度にその位置における最大加速度を加算した第１速度を、前記第１速度に対応する前記第２速度を上限として順次算出し、前記速度パターン作成手段は、前記動作開始位置から前記動作終了位置までの各位置において、前記第１速度算出手段により算出された前記第１速度を用いて前記速度パターンを作成する。

上記構成によれば、動作終了位置から動作開始位置まで第２速度を算出した後に、動作開始位置から動作終了位置まで第１速度を算出する場合に、第２の手段と同様の作用効果を奏することができる。

第４の手段では、前記動作開始位置側から前記動作終了位置側へ各位置における速度にその位置における最大減速度を加算した第３速度を算出する第３速度算出手段と、前記第３速度算出手段により算出された前記第３速度が、前記第３速度に対応する前記上限速度よりも高くなる位置である超過位置と、前記超過位置から前記第３速度に各位置における最大減速度を順次加算した補正速度が、前記補正速度に対応する前記上限速度よりも低くなる位置である復帰位置とを算出する位置算出手段と、を備え、前記第１速度算出手段は、前記超過位置から前記復帰位置まで前記第１速度の算出を停止し、前記第２速度算出手段は、前記復帰位置から前記動作開始位置側へ各位置における速度にその位置における最大減速度を加算した第２速度を、前記第２速度に対応する前記上限速度を上限として算出する。

上記構成によれば、第３速度算出手段により、動作開始位置側から動作終了位置側へ各位置における速度にその位置における最大減速度を加算した第３速度が算出される。そして、第３速度算出手段により算出された第３速度が、第３速度に対応する上限速度よりも高くなる位置である超過位置が算出される。この超過位置は、最大減速度で減速しても速度が上限速度よりも高くなる位置であり、最大速度条件を満たすことができなくなる位置である。また、超過位置から第３速度に各位置における最大減速度を順次加算した補正速度が、補正速度に対応する上限速度よりも低くなる位置である復帰位置が算出される。この復帰位置は、超過位置から最大減速度で順次減速した場合に、速度が上限速度よりも低くなる位置であり、最大速度条件を満たすことができる位置である。

そこで、第１速度算出手段は、超過位置から復帰位置まで第１速度の算出を停止する。これにより、第１速度が上限速度を超える範囲について、第１速度の算出を停止することができ、不要な計算を省略することができる。そして、第２速度算出手段により、復帰位置から動作開始位置側へ各位置における速度にその位置における最大減速度を加算した第２速度が、第２速度に対応する上限速度を上限として算出される。したがって、超過位置から復帰位置までの範囲についても、速度パターンを作成することができる。なお、速度パターン作成手段は、動作開始位置から動作終了位置までの各位置において、第１速度及び第２速度のいずれかが算出されていない場合は、算出されている方の速度を選択して速度パターンを作成する。

第５の手段では、前記動作終了位置側から前記動作開始位置側へ各位置における速度にその位置における最大加速度を加算した第４速度を算出する第４速度算出手段と、前記第４速度算出手段により算出された前記第４速度が、前記第４速度に対応する前記上限速度よりも高くなる位置である超過位置と、前記超過位置から前記第４速度に各位置における最大加速度を順次加算した補正速度が、前記補正速度に対応する前記上限速度よりも低くなる位置である復帰位置とを算出する位置算出手段と、を備え、前記第２速度算出手段は、前記超過位置から前記復帰位置まで前記第２速度の算出を停止し、前記第１速度算出手段は、前記復帰位置から前記動作終了位置側へ各位置における速度にその位置における最大加速度を加算した第１速度を、前記第１速度に対応する前記上限速度を上限として算出する。

上記構成によれば、動作終了位置側から動作開始位置側へ第４速度を算出した後に、復帰位置から動作終了位置側へ第１速度を算出する場合に、第４の手段と同様の作用効果を奏することができる。

第６の手段は、サーボモータにより駆動されるアームを備えるロボットの制御方法であって、前記サーボモータの動作開始位置から動作終了位置まで各位置における前記サーボモータの上限速度を算出する上限速度算出工程と、前記動作開始位置側から前記動作終了位置側へ各位置における速度にその位置における最大加速度を加算した第１速度を、前記第１速度に対応する前記上限速度を上限として算出する第１速度算出工程と、前記動作終了位置側から前記動作開始位置側へ各位置における速度にその位置における最大減速度を加算した第２速度を、前記第２速度に対応する前記上限速度を上限として算出する第２速度算出工程と、前記動作開始位置から前記動作終了位置までの各位置において、前記第１速度算出工程で算出された前記第１速度及び前記第２速度算出工程で算出された前記第２速度のうち低い方の速度を選択して前記サーボモータの速度パターンを作成する速度パターン作成工程と、を備えることを特徴とする。

上記工程によれば、第１の手段と同様の作用効果を奏することができる。

ロボットシステムの概要を示す斜視図。 第１実施形態にかかる最適の速度パターンを作成する処理の概要を示すフローチャート。 最適の速度パターンを作成する処理の概要を示すグラフ。 最適の速度パターンを作成する処理の詳細を示すフローチャート。 最大速度条件不満足の状態と、最大減速度条件に基づく第２速度算出とを示すグラフ。 最適の速度パターンを作成する処理の変更例の概要を示すフローチャート。 第２実施形態にかかる最適の速度パターンを作成する処理の概要を示すフローチャート。 最適の速度パターンを作成する処理の概要を示すグラフ。 最適の速度パターンを作成する処理の詳細を示すフローチャート。 最適の速度パターンを作成する処理の詳細を示すフローチャート。 最適の速度パターンを作成する処理の詳細を示すフローチャート。 最適の速度パターンを作成する処理の変更例の概要を示すフローチャート。

（第１実施形態）
以下、第１実施形態について、図面を参照しつつ説明する。本実施形態は、機械組立工場等において、機械等の組み立てを行うロボットシステムとして具体化している。

図１は、ロボットシステム１０の概要を示す斜視図である。同図に示すように、ロボットシステム１０は、ロボット２０及びロボットコントローラ３０を備えている。

ロボット２０は、垂直多関節型のロボットであり、６つ（複数）の関節を有するアーム２１、及び基台２２を備えている。アーム２１（被駆動部）は、先端にハンド部２１ａを有している。アーム２１の各関節には、それぞれサーボモータ（２５，２６のみ表示）が設けられており、これらのサーボモータの回転によりアーム２１が駆動される。各サーボモータには、その出力軸を制動する電磁ブレーキと、出力軸の回転角度に応じたパルス信号を出力するエンコーダとがそれぞれ設けられている。ロボット２０は、アーム２１を動作させることにより、ワークに対する部品の組付けやワークの搬送等の作業を行う。詳しくは、ロボット２０は、ベルトコンベア上を流れるワーク（部品）の位置を特定して、ワークをアーム２１で掴む動作を実行する。

コントローラ３０（制御装置）は、ＣＰＵ、ＲＯＭ、ＲＡＭ、データ記憶部、駆動回路、位置検出回路等を備えている。ＲＯＭは、ロボット２０のシステムプログラムや動作プログラム等を記憶している。ＲＡＭは、これらのプログラムを実行する際にパラメータの値等を記憶する。データ記憶部は、ワーク認識用のカメラ（図示せず）から入力される画像データ等を記憶する。位置検出回路には、各エンコーダの検出信号がそれぞれ入力される。位置検出回路は、各エンコーダの検出信号に基づいて、各関節に設けられたサーボモータの回転角度を検出する。ＣＰＵは、動作プログラム（プログラム）を実行することにより、位置検出回路から入力される位置情報に基づいて、アーム２１の各関節の回転角度を開始回転角度（動作開始位置）から終了回転角度（動作終了位置）まで制御する。本実施形態では、この制御に際して、各サーボモータの最適の速度パターンをリアルタイムで作成し、作成した速度パターンをロボット２０の自動運転時の動作において実現する。

次に、図２のフローチャート及び図３のグラフを参照して、各サーボモータの最適の速度パターンを作成する処理の概要を説明する。この一連の処理は、コントローラ３０によって実行される。

まず、最大速度条件に基づいて、動作開始位置から動作終了位置までの各位置における各サーボモータの上限速度ｖ０を算出する（Ｓ１１）。図３（ａ）に示すように、各サーボモータについて位置λに対する上限速度ｖ０を算出する。

続いて、動作開始位置から動作終了位置まで上限速度ｖ０を上限として、最大加速度で第１速度ｖ１を算出する（Ｓ１２）。図３（ｂ）に示すように、動作開始位置から各位置における最大加速度を加算して第１速度ｖ１を順次算出する。このとき、第１速度ｖ１が上限速度ｖ０未満でない場合は、第１速度ｖ１を上限速度ｖ０で制限し、第１速度ｖ１を上限速度ｖ０とする。

続いて、動作終了位置から動作開始位置まで第１速度ｖ１を上限として、最大減速度で第２速度ｖ２を算出する（Ｓ１３）。図３（ｃ）に示すように、動作終了位置から各位置における最大減速度を加算して第２速度ｖ２を順次算出する。最大減速度は、動作開始位置側から動作終了位置側へ移動する時の最大減速度であり、後退積分においては第２速度ｖ２の最大上昇量となる。このとき、第２速度ｖ２が第１速度ｖ１未満でない場合は、第２速度ｖ２を第１速度ｖ１（≦上限速度ｖ０）で制限し、第２速度ｖ２を第１速度ｖ１とする。すなわち、第１速度ｖ１及び第２速度ｖ２のうち低い方の速度を選択する。

続いて、動作開始位置から動作終了位置まで、各位置における第２速度ｖ２を用いて速度パターンを作成する（Ｓ１４）。そして、この一連の処理を終了する（ＥＮＤ）。

なお、Ｓ１１の処理が上限速度算出手段（工程）としての処理に相当し、Ｓ１２の処理が第１速度算出手段（工程）としての処理に相当し、Ｓ１３の処理が第２速度算出手段（工程）としての処理に相当し、Ｓ１４の処理が速度パターン作成手段（工程）としての処理に相当する。

次に、図４のフローチャートを参照して、各サーボモータの最適の速度パターンを作成する処理の詳細を説明する。この一連の処理は、コントローラ３０によって実行される。

まず、最大速度条件に基づいて、動作開始位置から動作終了位置までの各位置における各サーボモータの上限速度ｖ０を算出する（Ｓ２１）。最大速度条件として、各サーボモータの仕様により定まる各サーボモータの最大速度や、アーム２１の機械的特性により定まるアーム２１の最大速度等を超えないことが要求される。そして、最大速度条件を満たすように、各位置における各サーボモータの上限速度ｖ０を算出する。

続いて、現在位置から前進積分により最大加速度で第１速度ｖ１を算出する（Ｓ２２）。詳しくは、現在のステップ（現在位置）における第１速度ｖ１に現在のステップにおける最大加速度を加算して、次のステップにおける第１速度ｖ１を算出する。なお、前進積分開始時の現在のステップ（現在位置）はステップ０（動作開始位置：λ＝０）であり、ステップ０における第１速度ｖ１＝０である。

続いて、算出された第１速度ｖ１が、対応する上限速度ｖ０未満であるか否か判定する（Ｓ２３）。すなわち、共通のステップ（位置）において、第１速度ｖ１が上限速度ｖ０未満であるか否か判定する。この判定において、第１速度ｖ１が、対応する位置における上限速度ｖ０未満であると判定した場合（Ｓ２３：ＹＥＳ）、次のステップにおける第１速度ｖ１を、Ｓ２２で算出した第１速度ｖ１とする（Ｓ２４）。一方、Ｓ２３の判定において、第１速度ｖ１が対応する上限速度ｖ０未満でないと判定した場合（Ｓ２３：ＮＯ）、次のステップにおける第１速度ｖ１を対応する上限速度ｖ０とする（Ｓ２５）。すなわち、第１速度ｖ１を上限速度ｖ０で制限する。

続いて、前進積分のステップ（位置）を一つ進め（Ｓ２６）、前進積分の位置が動作終了位置に到達したか否か判定する（Ｓ２７）。この判定において、前進積分の位置が動作終了位置に到達していないと判定した場合（Ｓ２７：ＮＯ）、Ｓ２２〜Ｓ２６の処理を再度実行する。一方、Ｓ２７の判定において、前進積分の位置が動作終了位置に到達したと判定した場合（Ｓ２７：ＹＥＳ）、現在位置から後退積分により最大減速度で第２速度ｖ２を算出する（Ｓ２８）。詳しくは、現在のステップ（現在位置）における第２速度ｖ２に現在のステップにおける最大減速度（動作開始位置側への最大上昇量）を加算して、次のステップにおける第２速度ｖ２を算出する。なお、後退積分開始時の現在のステップ（現在位置）はステップ０（動作終了位置：λ＝１）であり、ステップ０における第２速度ｖ２＝０である。

続いて、算出された第２速度ｖ２が、対応する第１速度ｖ１未満であるか否か判定する（Ｓ２９）。すなわち、共通のステップ（位置）において、第２速度ｖ２が第１速度ｖ１未満であるか否か判定する。この判定において、第２速度ｖ２が、対応する位置における第１速度ｖ１未満であると判定した場合（Ｓ２９：ＹＥＳ）、次のステップにおける第２速度ｖ２を、Ｓ２８で算出した第２速度ｖ２とする（Ｓ３０）。一方、Ｓ２９の判定において、第２速度ｖ２が対応する第１速度ｖ１未満でないと判定した場合（Ｓ２９：ＮＯ）、次のステップにおける第２速度ｖ２を対応する第１速度ｖ１とする（Ｓ３１）。すなわち、第２速度ｖ２を第１速度ｖ１（≦上限速度ｖ０）で制限する。

続いて、後退積分のステップ（位置）を一つ進め（Ｓ３２）、後退積分の位置が動作開始位置に到達したか否か判定する（Ｓ３３）。この判定において、後退積分の位置が動作終了位置に到達していないと判定した場合（Ｓ３３：ＮＯ）、Ｓ２８〜Ｓ３２の処理を再度実行する。一方、Ｓ３３の判定において、後退積分の位置が動作開始位置に到達したと判定した場合（Ｓ３３：ＹＥＳ）、動作開始位置から動作終了位置まで、各位置における第２速度ｖ２を用いて速度パターンを作成する（Ｓ３４）。そして、この一連の処理を終了する（ＥＮＤ）。

なお、Ｓ２１の処理が上限速度算出手段（工程）としての処理に相当し、Ｓ２２〜Ｓ２７の処理が第１速度算出手段（工程）としての処理に相当し、Ｓ２８〜Ｓ３３の処理が第２速度算出手段（工程）としての処理に相当し、Ｓ３４の処理が速度パターン作成手段（工程）としての処理に相当する。

図５のグラフを参照して、最大速度条件不満足となる状態と、最大減速度条件に基づく第２速度ｖ２の算出とを説明する。

図５（ａ）に示すように、前進積分により第１速度ｖ１を算出する際に、最大減速度で減速しても第１速度ｖ１が上限速度ｖ０を超える点Ｐ１が生じる場合がある。この場合、点Ｐ１において、第１速度ｖ１が最大速度条件を満たすことができなくなる。

この点、本実施形態によれば、図５（ｂ）に示すように、後退積分により最大減速度で第２速度ｖ２が算出される。第２速度ｖ２は、点Ｐ２において第１速度ｖ１（＝上限速度ｖ０）まで上昇することができなくなる。このため、点Ｐ２から点Ｐ３まで、最大減速度で算出された第２速度ｖ２が選択されて速度パターンが作成される。その結果、速度パターンは最大加減速度条件を満たすとともに、最大速度条件を満たさなくなる点Ｐ１を避けることができる。

以上詳述した本実施形態は、以下の利点を有する。

・動作開始位置側から動作終了位置側へ各位置における速度にその位置における最大加速度を加算した第１速度ｖ１が算出される。ただし、第１速度ｖ１に対応する上限速度ｖ０が上限とされるため、第１速度ｖ１が上限速度ｖ０を超える場合には、第１速度ｖ１は上限速度ｖ０に制限される。このため、第１速度ｖ１は、上限速度ｖ０を超えない範囲で最大の速度として算出され、最大速度条件を満たすことができる。

・動作終了位置側から動作開始位置側へ各位置における速度にその位置における最大減速度を加算した第２速度ｖ２が算出される。ただし、第２速度ｖ２に対応する上限速度ｖ０が上限とされるため、第２速度ｖ２が上限速度ｖ０を超える場合には、第２速度ｖ２は上限速度ｖ０に制限される。このため、第２速度ｖ２は、上限速度ｖ０を超えない範囲で、最大の速度として算出され、最大速度条件を満たすことができる。

・共通の位置（ステップ）において、第１速度ｖ１よりも第２速度ｖ２が低い場合は、第１速度ｖ１はその位置での最大減速度条件を満たさないこととなる。この点、動作開始位置から動作終了位置までの各位置において、第１速度ｖ１及び第２速度ｖ２のうち低い方の速度が選択されてサーボモータの速度パターンが作成される。このため、この速度パターンは、最大加減速度条件を満たすことができる。したがって、速度パターンを簡易な処理で最適化することができ、最適の速度パターンをリアルタイムで作成することが可能となる。

・動作開始位置から動作終了位置まで各位置における速度にその位置における最大加速度を加算した第１速度ｖ１が、第１速度ｖ１に対応する上限速度ｖ０を上限として順次算出される。すなわち、動作開始位置から動作終了位置までの第１速度ｖ１パターンが作成される。この第１速度ｖ１パターンは、上限速度ｖ０を超えない範囲で最大の速度として算出され、最大速度条件を満たすことができる。

・動作終了位置から動作開始位置まで各位置における速度にその位置における最大減速度を加算した第２速度ｖ２が、第２速度ｖ２に対応する第１速度ｖ１を上限として順次算出される。すなわち、動作終了位置から動作開始位置までの第２速度ｖ２パターンが作成される。この第２速度ｖ２パターンは、上限速度ｖ０を超えない範囲で最大の速度として算出された第１速度ｖ１を上限としており、最大速度条件を満たすことができる。

・動作開始位置から動作終了位置までの各位置において、第２速度ｖ２を用いて速度パターンが作成される。したがって、共通の位置において、第１速度ｖ１よりも第２速度ｖ２が低い場合は、第２速度ｖ２が選択されて速度パターンが作成されることとなる。このため、この速度パターンは、最大加減速度条件を満たすことができる。

なお、上記第１実施形態を、以下のように変更して実施することもできる。

・図６は、各サーボモータの最適の速度パターンを作成する処理の変更例の概要を示すフローチャートである。ここでは、第１速度ｖ１と第２速度ｖ２との算出順序を、第１実施形態と逆にしている。この一連の処理は、コントローラ３０によって実行される。

まず、最大速度条件に基づいて、動作開始位置から動作終了位置までの各位置における各サーボモータの上限速度ｖ０を算出する（Ｓ１１Ａ）。動作終了位置から動作開始位置まで上限速度ｖ０を上限として、最大減速度で第２速度ｖ２を算出する（Ｓ１２Ａ）。動作開始位置から動作終了位置まで第２速度ｖ２を上限として、最大加速度で第１速度ｖ１を算出する（Ｓ１３Ａ）。動作開始位置から動作終了位置まで、各位置における第１速度ｖ１を用いて速度パターンを作成する（Ｓ１４Ａ）。そして、この一連の処理を終了する（ＥＮＤ）。

なお、Ｓ１１Ａの処理が上限速度算出手段（工程）としての処理に相当し、Ｓ１２Ａの処理が第２速度算出手段（工程）としての処理に相当し、Ｓ１３Ａの処理が第１速度算出手段（工程）としての処理に相当し、Ｓ１４Ａの処理が速度パターン作成手段（工程）としての処理に相当する。

そして、各サーボモータの最適の速度パターンを作成する処理の詳細は、図６のフローチャートの処理に沿って図４のフローチャートの処理を変更すればよい。こうした構成によれば、動作終了位置から動作開始位置まで第２速度ｖ２を算出した後に、動作開始位置から動作終了位置まで第１速度ｖ１を算出する場合に、第１実施形態と同様の作用効果を奏することができる。

・第１速度ｖ１と第２速度ｖ２とを並列に算出し、各位置において第１速度ｖ１及び第２速度ｖ２のうち低い方の速度を選択して速度パターンを作成してもよい。こうした構成によっても、第１実施形態と同様の作用効果を奏することができる。

（第２実施形態）
以下、第２実施形態について、図面を参照しつつ説明する。本実施形態では、図５に示したように、最大速度条件を満たさなくなる点Ｐ１（超過位置）から、最大速度条件を満たす点Ｐ２（復帰位置）まで、第１速度ｖ１を算出することなく第２速度ｖ２を算出する。

図７のフローチャート及び図８のグラフを参照して、各サーボモータの最適の速度パターンを作成する処理の概要を説明する。この一連の処理は、コントローラ３０によって実行される。

まず、最大速度条件に基づいて、動作開始位置から動作終了位置までの各位置における各サーボモータの上限速度ｖ０を算出する（Ｓ４１）。図８（ａ）に示すように、各サーボモータについて位置λに対する上限速度ｖ０を算出する。

続いて、動作開始位置から動作終了位置側へ上限速度ｖ０を上限として、最大加速度で第１速度ｖ１を算出する（Ｓ４２）。図８（ｂ）に示すように、動作開始位置から各位置における最大加速度を加算して第１速度ｖ１を順次算出する。このとき、第１速度ｖ１が上限速度ｖ０未満でない場合は、第１速度ｖ１を上限速度ｖ０で制限し、第１速度ｖ１を上限速度ｖ０とする。

続いて、最大速度条件を満たさなくなる超過位置（点Ｐ１）、及び最大速度条件を満たす復帰位置（点Ｐ２）を算出する（Ｓ４３）。図８（ｃ）に示すように、前進積分により第１速度ｖ１を算出する際に、最大減速度で減速しても第１速度ｖ１が上限速度ｖ０を超える点Ｐ１を算出する。そして、図８（ｄ）に示すように、点Ｐ１から最大減速度で減速を続けて、速度が上限速度ｖ０未満となる点Ｐ２を算出する。

続いて、復帰位置から動作開始位置側へ上限速度ｖ０を上限として、最大減速度で第２速度ｖ２を算出する（Ｓ４４）。図８（ｅ）に示すように、点Ｐ２から動作開始位置側へ各位置における最大減速度を加算して第２速度ｖ２を順次算出する。このとき、第２速度ｖ２が上限速度ｖ０未満でない場合は、第２速度ｖ２を上限速度ｖ０で制限し、第２速度ｖ２を上限速度ｖ０とする。第２速度ｖ２の算出は、第２速度ｖ２が第１速度ｖ１よりも高くなるまで行う。

続いて、復帰位置（点Ｐ２）から動作終了位置まで上限速度ｖ０を上限として、最大加速度で第１速度ｖ１を算出する（Ｓ４５）。図８（ｆ）に示すように、点Ｐ２から各位置における最大加速度を加算して第１速度ｖ１を順次算出する。このとき、第１速度ｖ１が上限速度ｖ０未満でない場合は、第１速度ｖ１を上限速度ｖ０で制限し、第１速度ｖ１を上限速度ｖ０とする。

続いて、動作終了位置から動作開始位置側へ第１速度ｖ１を上限として、最大減速度で第２速度ｖ２を算出する（Ｓ４６）。図８（ｆ）に示すように、動作終了位置から各位置における最大減速度を加算して第２速度ｖ２を順次算出する。このとき、第２速度ｖ２が第１速度ｖ１未満でない場合は、第２速度ｖ２を第１速度ｖ１（≦上限速度ｖ０）で制限し、第２速度ｖ２を第１速度ｖ１とする。すなわち、第１速度ｖ１及び第２速度ｖ２のうち低い方の速度を選択する。

続いて、動作開始位置から動作終了位置まで、各位置において第１速度ｖ１及び第２速度ｖ２のいずれかを用いて速度パターンを作成する（Ｓ４７）。そして、この一連の処理を終了する（ＥＮＤ）。

なお、Ｓ４１の処理が上限速度算出手段（工程）としての処理に相当し、Ｓ４２，Ｓ４５の処理が第１速度算出手段（工程）としての処理に相当し、Ｓ４３の処理が位置算出手段としての処理に相当し、Ｓ４４，Ｓ４６の処理が第２速度算出手段（工程）としての処理に相当し、Ｓ４７の処理が速度パターン作成手段（工程）としての処理に相当する。

次に、図９〜１１のフローチャートを参照して、各サーボモータの最適の速度パターンを作成する処理の詳細を説明する。この一連の処理は、コントローラ３０によって実行される。

まず、最大速度条件に基づいて、動作開始位置から動作終了位置までの各位置における各サーボモータの上限速度ｖ０を算出する（Ｓ５１）。現在位置から前進積分により最大加速度で第１速度ｖ１を算出する（Ｓ５２）。なお、前進積分開始時の現在のステップ（現在位置）はステップ０（動作開始位置：λ＝０）であり、ステップ０における第１速度ｖ１＝０である。算出された第１速度ｖ１が、対応する上限速度ｖ０未満であるか否か判定する（Ｓ５３）。この判定において、第１速度ｖ１が対応する上限速度ｖ０未満であると判定した場合（Ｓ５３：ＹＥＳ）、次のステップにおける第１速度ｖ１を、Ｓ５２で算出した第１速度ｖ１とする（Ｓ５４）。Ｓ５１〜Ｓ５４の処理は、図４のＳ２１〜Ｓ２４の処理と同一である。

一方、Ｓ５３の判定において、第１速度ｖ１が対応する上限速度ｖ０未満でないと判定した場合（Ｓ５３：ＮＯ）、第３速度ｖ３を算出し、算出した第３速度ｖ３が対応する上限速度ｖ０未満であるか否か判定する（Ｓ５５）。第３速度ｖ３は、現在位置から前進積分により最大減速度で算出した速度である。この判定において、第３速度ｖ３が、対応する位置における上限速度ｖ０未満であると判定した場合（Ｓ５５：ＹＥＳ）、次のステップにおける第１速度ｖ１を対応する上限速度ｖ０とする（Ｓ５６）。すなわち、第１速度ｖ１を上限速度ｖ０で制限する。

続いて、前進積分のステップ（位置）を一つ進め（Ｓ５７）、前進積分の位置が動作終了位置に到達したか否か判定する（Ｓ５８）。この判定において、前進積分の位置が動作終了位置に到達していないと判定した場合（Ｓ５８：ＮＯ）、Ｓ５２〜Ｓ５７の処理を再度実行する。一方、Ｓ５８の判定において、前進積分の位置が動作終了位置に到達したと判定した場合（Ｓ５８：ＹＥＳ）、図１１の（３）終了位置からの処理へ進む。（３）終了位置からの処理については後述する。

また、Ｓ５５の判定において、第３速度ｖ３が対応する上限速度ｖ０未満でないと判定した場合（Ｓ５５：ＮＯ）、図１０の（１）超過位置からの処理へ進む。そして、前進積分のステップ（位置）を一つ進めて、補正速度ｖｒを算出する（Ｓ５９）。補正速度ｖｒは、超過位置（点Ｐ１）から第３速度ｖ３に各位置における最大減速度を加算した速度である。すなわち、補正速度ｖｒ（初期値は第３速度ｖ３）を最大減速度で前進積分する。

続いて、補正速度ｖｒが、対応する上限速度ｖ０未満であるか否か判定する（Ｓ６０）。すなわち、補正速度ｖｒが、最大速度条件を満たす速度（上限速度ｖ０）まで低下したか否か判定する。この判定において、補正速度ｖｒが、対応する位置における上限速度ｖ０未満でないと判定した場合（Ｓ６０：ＮＯ）、Ｓ５９の処理を再度実行する。

一方、Ｓ６０の判定において、補正速度ｖｒが対応する上限速度ｖ０未満であると判定した場合（Ｓ６０：ＹＥＳ）、次のステップにおける第１速度ｖ１を対応する上限速度ｖ０とする（Ｓ６１）。この位置が、図５（ｂ）における点Ｐ２（復帰位置）となる。

続いて、現在位置から後退積分により最大減速度で第２速度ｖ２を算出する（Ｓ６２）。詳しくは、現在のステップ（現在位置）における第２速度ｖ２に現在のステップにおける最大減速度（動作開始位置側への最大上昇量）を加算して、次のステップにおける第２速度ｖ２を算出する。なお、後退積分開始時の現在のステップ（現在位置）はステップ０（復帰位置）であり、ステップ０における第２速度ｖ２＝上限速度ｖ０である。

続いて、後退積分により第２速度ｖ２を算出した位置に、既に算出されている第１速度ｖ１が存在するか否か判定する（Ｓ６３）。すなわち、図５（ｂ）において、点Ｐ１（超過位置）から点Ｐ２（復帰位置）までは第１速度ｖ１が算出されておらず、第２速度ｖ２の後退積分が点Ｐ１（超過位置）まで進んだか否か判定する。この判定において、後退積分により第２速度ｖ２を算出した位置に、既に算出されている第１速度ｖ１が存在しないと判定した場合（Ｓ６３：ＮＯ）、次のステップにおける第２速度ｖ２を、Ｓ６２で算出した第２速度ｖ２とする（Ｓ６５）。そして、後退積分のステップ（位置）を一つ進め（Ｓ６６）、再度Ｓ６２の処理を実行する。

一方、Ｓ６３の判定において、後退積分により第２速度ｖ２を算出した位置に、既に算出されている第１速度ｖ１が存在すると判定した場合（Ｓ６３：ＹＥＳ）、算出された第２速度ｖ２が第１速度ｖ１を超えているか否か判定する（Ｓ６４）。すなわち、図５（ｂ）において、点Ｐ１（超過位置）から点Ｐ３（交差位置）までは第２速度ｖ２は第１速度ｖ１未満であり、第２速度ｖ２が第１速度ｖ１を超える点Ｐ３まで到達したか否か判定する。この判定において、算出された第２速度ｖ２が第１速度ｖ１を超えていないと判定した場合（Ｓ６４：ＮＯ）、Ｓ６５の処理から再度実行する。

一方、Ｓ６４の判定において、算出された第２速度ｖ２が第１速度ｖ１を超えていると判定した場合（Ｓ６４：ＹＥＳ）、図９の（２）復帰位置からの処理へ進む。すなわち、図５（ｂ）の点Ｐ２（復帰位置）から、Ｓ５２〜Ｓ５８の処理により再度第１速度ｖ１を算出する。

また、図１１の（３）終了位置からの処理へ進んだ場合は、現在位置から後退積分により最大減速度で第２速度ｖ２を算出する（Ｓ６７）。詳しくは、現在のステップ（現在位置）における第２速度ｖ２に現在のステップにおける最大減速度（動作開始位置側への最大上昇量）を加算して、次のステップにおける第２速度ｖ２を算出する。なお、後退積分開始時の現在のステップ（現在位置）はステップ０（動作終了位置：λ＝１）であり、ステップ０における第２速度ｖ２＝０である。

続いて、算出された第２速度ｖ２が、対応する第１速度ｖ１を超えているか否か判定する（Ｓ６８）。この判定において、第２速度ｖ２が対応する第１速度ｖ１を超えていないと判定した場合（Ｓ６８：ＮＯ）、次のステップにおける第２速度ｖ２を、Ｓ６７で算出した第２速度ｖ２とする（Ｓ６９）。そして、後退積分のステップ（位置）を一つ進め（Ｓ７０）、Ｓ６７の処理から再度実行する。

一方、Ｓ６８の判定において、第２速度ｖ２が対応する第１速度ｖ１超えていると判定した場合（Ｓ６８：ＹＥＳ）、動作開始位置から動作終了位置まで各位置において、第１速度ｖ１及び第２速度ｖ２のうち低い方の速度を選択して速度パターンを作成する（Ｓ７１）。そして、この一連の処理を終了する（ＥＮＤ）。

なお、Ｓ５１の処理が上限速度算出手段（工程）としての処理に相当し、Ｓ５２〜Ｓ５８の処理が第１速度算出手段（工程）としての処理に相当し、Ｓ５５の処理が第３速度算出手段及び位置算出手段としての処理に相当し、Ｓ６２〜Ｓ６６，Ｓ６７〜Ｓ７０の処理が第２速度算出手段（工程）としての処理に相当し、Ｓ５９〜Ｓ６１の処理が位置算出手段としての処理に相当し、Ｓ７１の処理が速度パターン作成手段（工程）としての処理に相当する。

以上詳述した本実施形態は、以下の利点を有する。

・動作開始位置側から動作終了位置側へ各位置における速度にその位置における最大減速度を加算した第３速度ｖ３が算出される。そして、算出された第３速度ｖ３が、第３速度ｖ３に対応する上限速度ｖ０よりも高くなる位置である超過位置（点Ｐ１）が算出される。これにより、最大速度条件を満たすことができなくなる位置である超過位置を算出することができる。

・超過位置から第３速度ｖ３に各位置における最大減速度を順次加算した補正速度ｖｒが、補正速度ｖｒに対応する上限速度ｖ０よりも低くなる位置である復帰位置（点Ｐ２）が算出される。これにより、最大速度条件を満たすことができる位置である復帰位置を算出することができる。

・超過位置から復帰位置まで第１速度ｖ１の算出が停止される。このため、第１速度ｖ１が上限速度ｖ０を超える範囲について、第１速度ｖ１の算出を停止することができ、不要な計算を省略することができる。

・復帰位置から動作開始位置側へ各位置における速度にその位置における最大減速度を加算した第２速度ｖ２が、第２速度ｖ２に対応する上限速度ｖ０を上限として算出される。したがって、超過位置から復帰位置までの範囲についても、速度パターンを作成することができる。

なお、上記第２実施形態を、以下のように変更して実施することもできる。

・図７のフローチャートにおいて、復帰位置から動作開始位置側へ第２速度ｖ２を算出する処理（Ｓ４４）と、復帰位置から動作終了位置側へ第１速度ｖ１を算出する処理（Ｓ４５）とは、どちらの処理を先に行ってもよく、また両処理を並列で行ってもよい。

・図１２は、各サーボモータの最適の速度パターンを作成する処理の変更例の概要を示すフローチャートである。ここでは、第１速度ｖ１と第２速度ｖ２との算出順序を、第２実施形態と逆にしている。この一連の処理は、コントローラ３０によって実行される。

まず、最大速度条件に基づいて、動作開始位置から動作終了位置までの各位置における各サーボモータの上限速度ｖ０を算出する（Ｓ４１Ａ）。動作終了位置から動作開始位置側へ上限速度ｖ０を上限として、最大減速度で第２速度ｖ２を算出する（Ｓ４２Ａ）。最大速度条件を満たさなくなる超過位置、及び最大速度条件を満たす復帰位置を算出する（Ｓ４３Ａ）。復帰位置から動作終了位置側へ上限速度ｖ０を上限として、最大加速度で第１速度ｖ１を算出する（Ｓ４４Ａ）。復帰位置から動作開始位置まで上限速度ｖ０を上限として、最大減速度で第２速度ｖ２を算出する（Ｓ４５Ａ）。動作開始位置から動作終了位置側へ第２速度ｖ２を上限として、最大加速度で第１速度ｖ１を算出する（Ｓ４６Ａ）。動作開始位置から動作終了位置まで、各位置において第１速度ｖ１及び第２速度ｖ２のいずれかを用いて速度パターンを作成する（Ｓ４７Ａ）。そして、この一連の処理を終了する（ＥＮＤ）。

なお、Ｓ４１Ａの処理が上限速度算出手段（工程）としての処理に相当し、Ｓ４２Ａ，Ｓ４５Ａの処理が第２速度算出手段（工程）としての処理に相当し、Ｓ４３Ａの処理が位置算出手段としての処理に相当し、Ｓ４４Ａ，Ｓ４６Ａの処理が第１速度算出手段（工程）としての処理に相当し、Ｓ４７Ａの処理が速度パターン作成手段（工程）としての処理に相当する。

そして、各サーボモータの最適の速度パターンを作成する処理の詳細は、図１２のフローチャートの処理に沿って図９〜１１のフローチャートの処理を変更すればよい。こうした構成によれば、動作終了位置から動作開始位置側へ第２速度ｖ２を算出した後に、動作開始位置から動作終了位置側へ第１速度ｖ１を算出する場合に、第２実施形態と同様の作用効果を奏することができる。

・図１２のフローチャートにおいて、復帰位置から動作終了位置側へ第１速度ｖ１を算出する処理（Ｓ４４Ａ）と、復帰位置から動作開始位置まで第２速度ｖ２を算出する処理（Ｓ４５Ａ）とは、どちらの処理を先に行ってもよく、また両処理を並列で行ってもよい。

なお、上記各実施形態を、以下のように変更して実施することもできる。

・ロボット２０のアーム２１の関節（サーボモータ）の数は、６つに限らず任意である。

・垂直多関節型のロボット２０に代えて、水平多関節型のロボット等を採用することもできる。

２０…ロボット、２１…アーム、２５，２６…サーボモータ、３０…コントローラ。

Claims (6)

1. サーボモータにより駆動されるアームを備えるロボットの制御装置であって、
   前記サーボモータの動作開始位置から動作終了位置まで各位置における前記サーボモータの上限速度を算出する上限速度算出手段と、
   前記動作開始位置から前記動作終了位置まで各位置における速度にその位置における最大加速度を加算した第１速度を、前記第１速度に対応する前記上限速度を上限として算出する第１速度算出手段と、
   前記動作終了位置から前記動作開始位置まで各位置における速度にその位置における最大減速度を加算した第２速度を、前記第２速度に対応する前記上限速度を上限として算出する第２速度算出手段と、
   前記動作開始位置から前記動作終了位置までの各位置において、前記第１速度算出手段により算出された前記第１速度及び前記第２速度算出手段により算出された前記第２速度のうち低い方の速度を選択して前記サーボモータの速度パターンを作成する速度パターン作成手段と、
   を備えることを特徴とするロボットの制御装置。
2. サーボモータにより駆動されるアームを備えるロボットの制御装置であって、
   前記サーボモータの動作開始位置から動作終了位置まで各位置における前記サーボモータの上限速度を算出する上限速度算出手段と、
   前記動作開始位置から前記動作終了位置まで各位置における速度にその位置における最大加速度を加算した第１速度を、前記第１速度に対応する前記上限速度を上限として順次算出する第１速度算出手段と、
   前記動作終了位置から前記動作開始位置まで各位置における速度にその位置における最大減速度を加算した第２速度を、前記第２速度に対応する前記第１速度を上限として順次算出する第２速度算出手段と、
   前記動作開始位置から前記動作終了位置までの各位置において、前記第２速度算出手段により算出された前記第２速度を用いて速度パターンを作成する速度パターン作成手段と、
   を備えることを特徴とするロボットの制御装置。
3. サーボモータにより駆動されるアームを備えるロボットの制御装置であって、
   前記サーボモータの動作開始位置から動作終了位置まで各位置における前記サーボモータの上限速度を算出する上限速度算出手段と、
   前記動作終了位置から前記動作開始位置まで各位置における速度にその位置における最大減速度を加算した第２速度を、前記第２速度に対応する前記上限速度を上限として順次算出する第２速度算出手段と、
   前記動作開始位置から前記動作終了位置まで各位置における速度にその位置における最大加速度を加算した第１速度を、前記第１速度に対応する前記第２速度を上限として順次算出する第１速度算出手段と、
   前記動作開始位置から前記動作終了位置までの各位置において、前記第１速度算出手段により算出された前記第１速度を用いて速度パターンを作成する速度パターン作成手段と、
   を備えることを特徴とするロボットの制御装置。
4. サーボモータにより駆動されるアームを備えるロボットの制御装置であって、
   前記サーボモータの動作開始位置から動作終了位置まで各位置における前記サーボモータの上限速度を算出する上限速度算出手段と、
   前記動作開始位置から前記動作終了位置まで各位置における速度にその位置における最大加速度を加算した第１速度を、前記第１速度に対応する前記上限速度を上限として算出する第１速度算出手段と、
   前記動作終了位置から前記動作開始位置まで各位置における速度にその位置における最大減速度を加算した第２速度を、前記第２速度に対応する前記上限速度を上限として算出する第２速度算出手段と、
   前記動作開始位置側から前記動作終了位置側へ各位置における速度にその位置における最大減速度を加算した第３速度を算出する第３速度算出手段と、
   前記第３速度算出手段により算出された前記第３速度が、前記第３速度に対応する前記上限速度よりも高くなる位置である超過位置と、前記超過位置から前記第３速度に各位置における最大減速度を順次加算した補正速度が、前記補正速度に対応する前記上限速度よりも低くなる位置である復帰位置とを算出する位置算出手段と、
   前記動作開始位置から前記動作終了位置までの各位置において、前記第１速度算出手段により算出された前記第１速度及び前記第２速度算出手段により算出された前記第２速度のうち低い方の速度を選択して前記サーボモータの速度パターンを作成する速度パターン作成手段と、
   を備え、
   前記第１速度算出手段は、前記超過位置から前記復帰位置まで前記第１速度の算出を停止することを特徴とするロボットの制御装置。
5. サーボモータにより駆動されるアームを備えるロボットの制御装置であって、
   前記サーボモータの動作開始位置から動作終了位置まで各位置における前記サーボモータの上限速度を算出する上限速度算出手段と、
   前記動作開始位置から前記動作終了位置まで各位置における速度にその位置における最大加速度を加算した第１速度を、前記第１速度に対応する前記上限速度を上限として算出する第１速度算出手段と、
   前記動作終了位置から前記動作開始位置まで各位置における速度にその位置における最大減速度を加算した第２速度を、前記第２速度に対応する前記上限速度を上限として算出する第２速度算出手段と、
   前記動作終了位置側から前記動作開始位置側へ各位置における速度にその位置における最大加速度を加算した第４速度を算出する第４速度算出手段と、
   前記第４速度算出手段により算出された前記第４速度が、前記第４速度に対応する前記上限速度よりも高くなる位置である超過位置と、前記超過位置から前記第４速度に各位置における最大加速度を順次加算した補正速度が、前記補正速度に対応する前記上限速度よりも低くなる位置である復帰位置とを算出する位置算出手段と、
   前記動作開始位置から前記動作終了位置までの各位置において、前記第１速度算出手段により算出された前記第１速度及び前記第２速度算出手段により算出された前記第２速度のうち低い方の速度を選択して前記サーボモータの速度パターンを作成する速度パターン作成手段と、
   を備え、
   前記第２速度算出手段は、前記超過位置から前記復帰位置まで前記第２速度の算出を停止することを特徴とするロボットの制御装置。
6. サーボモータにより駆動されるアームを備えるロボットを制御装置により制御する制御方法であって、
   前記制御装置は、
   前記サーボモータの動作開始位置から動作終了位置まで各位置における前記サーボモータの上限速度を算出する上限速度算出工程と、
   前記動作開始位置から前記動作終了位置まで各位置における速度にその位置における最大加速度を加算した第１速度を、前記第１速度に対応する前記上限速度を上限として算出する第１速度算出工程と、
   前記動作終了位置から前記動作開始位置まで各位置における速度にその位置における最大減速度を加算した第２速度を、前記第２速度に対応する前記上限速度を上限として算出する第２速度算出工程と、
   前記動作開始位置から前記動作終了位置までの各位置において、前記第１速度算出工程で算出された前記第１速度及び前記第２速度算出工程で算出された前記第２速度のうち低い方の速度を選択して前記サーボモータの速度パターンを作成する速度パターン作成工程と、
   を実行することを特徴とするロボットの制御方法。

Images