JP5231935B2 - ロボット制御装置 - Google Patents

Description

本発明は、ロボットアーム等の目標位置決め制御を行うロボット制御装置に関する。

ロボットのアーム先端部等の制御対象（以下、「先端」という。）が直線や円弧あるいは指定された曲率の曲線等の軌道を描くように動作させる補間動作は、動作開始位置と最終目標位置に関しての位置と姿勢のデータ（以下、「先端位置指令値」という。）をロボット制御装置に記憶させて、その２点間において指定された軌道になるような補間された先端位置指令値を制御周期毎に算出し、これを各関節軸の角度（以下、「関節角度」という。）に変換して関節角度指令値として、さらに関節角度が関節角度指令値に一致するように各軸のアクチュエータを制御することによって実現している。

多関節型ロボットの場合、先端位置指令値から関節角度指令値へ変換できない場合があり、これを特異点と呼んでいる。ロボットが特異点や特異点近傍を通過すると、ある関節角速度が急激に非常に大きな値をとることが知られている。ところが実際には構成部品等の制限により、許容される最大速度が決まっており、最大速度を超過しないような制御処理、すなわち指令値生成が必要となる。従来においては特異点近傍であるかどうかの判定を行った上で、関節角速度が許容最大値を超えないような先端位置指令値を修正するものである（例えば、特許文献１及び２参照。）。

特許文献１では、座標変換して得られた関節角度指令値から特異点近傍かどうかを判定し、近傍であれば別処理によって関節角度指令値を生成している。特許文献２では、算出された関節角度指令値から関節角速度を計算し、この増加量に基づいて特異点近傍かどうかを判定し、位置指令値の速度を補正して最大速度を超えないようにしている。

しかし、特許文献１のような関節角度の特異点領域情報に基づいた特異点近傍判定方法では、あらかじめ全ての特異点領域を調べておく必要がある。また特異点近傍であっても制御点の動作速度が充分小さければ、最大速度を超えるにいたらないが、別処理を使用する必要があり、処理の切替時において速度を滑らかに変化させるといった新たな処理を必要とする。

また、特許文献２のような方法は、通常の動作において誤判定しないようにするために判定基準となる速度閾値を最大速度に設定しておく必要があり、そのため短時間とはいえ最大速度付近で動作させてしまう可能性がある。また速度を抑制させても過大判定直後においてはある程度の加速度を持っていることが多いため、すぐに減速することができない。

このように、特異点近傍のような関節角速度が急変するような領域をロボットが通過する場合にロボットを安定して動作させることが困難であった。
特開２００６−２２７７２４号公報 特開平６−３２４７３０号公報

本発明の目的は、特異点近傍のような関節角速度が急変するような領域を通過する場合であってもロボットを安定して動作させることができるロボット制御装置を提供することである。

本願発明の一態様によれば、関節を有するロボットを制御するロボット制御装置において、（イ）関節を制御周期毎に駆動するアクチュエータと、（ロ）関節の関節角度を制御周期毎に検出する検出部と、（ハ）関節角度から、ロボットの先端位置を制御周期毎に算出する位置算出部と、（ニ）先端位置を制御するための先端位置指令値を制御周期毎に算出する位置指令値算出部と、（ホ）先端位置と先端位置指令値との誤差を制御周期毎に算出する誤差算出部と、（ヘ）誤差から逆運動学計算により関節角差分を制御周期毎に算出する第１の関節角差分算出部と、（ト）関節角差分を積分することにより関節角度指令値を制御周期毎に算出する角度指令値算出部と、（チ）関節角度指令値に基づいてアクチュエータを制御周期毎に駆動させる駆動部と、（リ）検出部により検出された関節角度から関節角加速度を制御周期毎に算出することにより実行後の複数の制御周期における関節角加速度を算出し、実行後の複数の制御周期における関節角加速度から、実行前の制御周期における関節角速度を予測する関節角速度予測部と、（ヌ）実行前の制御周期における関節角速度に対する最大速度を記憶する最大速度記憶装置と、（ル）実行前の制御周期における関節角速度が最大速度を超えるか判定する速度判定部と、（ヲ）実行前の制御周期における関節角速度が最大速度を超えると判定された場合、実行前の制御周期における関節角速度から、実行前の制御周期における抑制関節角速度をそれぞれ算出する抑制速度算出部とを備えるロボット制御装置が提供される。

本願発明の他の態様によれば、関節を有するロボットを制御するロボット制御装置において、（イ）関節を制御周期毎に駆動するアクチュエータと、（ロ）関節の関節角度を制御周期毎に検出する検出部と、（ハ）関節角度から、ロボットの先端位置を制御周期毎に算出する位置算出部と、（ニ）先端位置を制御するための先端位置指令値を制御周期毎に算出する位置指令値算出部と、（ホ）先端位置と先端位置指令値との誤差を制御周期毎に算出する誤差算出部と、（ヘ）誤差から逆運動学計算により関節角差分を制御周期毎に算出する第１の関節角差分算出部と、（ト）関節角差分を積分することにより関節角度指令値を制御周期毎に算出する角度指令値算出部と、（チ）関節角度指令値に基づいてアクチュエータを制御周期毎に駆動させる駆動部と、（リ）位置指令値算出部により算出された先端位置指令値から関節角差分を順次算出することにより実行前の複数の制御周期における関節角差分をそれぞれ算出し、実行前の複数の制御周期における関節角差分から、実行前の複数の制御周期における関節角速度をそれぞれ予測する関節角速度予測部と、（ヌ）実行前の制御周期における関節角速度に対する最大速度を記憶する最大速度記憶装置と、（ル）実行前の複数の制御周期における関節角速度が最大速度を超えるかそれぞれ判定する速度判定部と、（ヲ）実行前の制御周期における関節角速度が最大速度を超えると判定された場合、最大速度を超えると判定された実行前の制御周期における関節角速度から、最大速度を超えると判定された実行前の制御周期における抑制関節角速度を算出する抑制速度算出部とを備えるロボット制御装置が提供される。

本発明によれば、特異点近傍のような関節角速度が急変するような領域を通過する場合であってもロボットを安定して動作させることができるロボット制御装置を提供することができる。

次に、図面を参照して、本発明の第１及び第２の実施の形態を説明する。以下の図面の記載において、同一又は類似の部分には同一又は類似の符号を付している。ただし、図面は模式的なものであることに留意すべきである。

また、以下に示す第１及び第２の実施の形態は、この発明の技術的思想を具体化するための装置や方法を例示するものであって、この発明の技術的思想は、構成部品の配置等を下記のものに特定するものでない。この発明の技術的思想は、特許請求の範囲において、種々の変更を加えることができる。

（第１の実施の形態）
本発明の第１の実施の形態に係るロボット制御装置は、中央演算処理装置（ＣＰＵ）１００、駆動部（アンプ）１０１、検出部１０２、アクチュエータ１２０、リンクパラメータ記憶装置２００、位置データ記憶装置２０１、加速度記憶装置２０２、及び最大速度記憶装置２０３を備える。

アクチュエータ１２０は、関節１２１に取り付けられ、関節１２１を制御周期毎に駆動する。関節１２１は、複数の関節及び複数のリンクを有するロボットの関節の一つである。

検出部１０２は、駆動部１０１に取り付けられている。検出部１０２は、エンコーダ等の位置検出センサ及びフィルタ等を備える。検出部１０２は、関節１２１の関節角度を制御周期毎に順次検出する。

ＣＰＵ１００は、位置算出部１０３、位置指令値算出部１０４、誤差算出部１０５、関節角差分算出部１０６、角度指令値算出部１０７、関節角速度予測部１１５、速度判定部１１３及び抑制速度算出部１１４をハードウェア資源であるモジュール（論理回路）として論理的に備える。

位置算出部１０３は、検出部１０２により検出された関節角度及びリンクパラメータ記憶装置２００に記憶されているリンクパラメータから、順運動学計算により直交座標系におけるロボットの先端位置を制御周期毎に順次算出する。

位置指令値算出部１０４は、位置データ記憶装置２０１に記憶された目標先端位置データから、ロボットの先端位置が動作開始位置から最終目標位置へ移動するように補間された先端位置指令値を制御周期毎に順次算出する。

誤差算出部１０５は、位置算出部１０３により算出されたロボットの先端位置と、位置指令値算出部１０４により算出された先端位置指令値との誤差を制御周期毎に順次算出する。

関節角差分算出部１０６は、誤差算出部１０５により算出された誤差から、ヤコビ行列を用いた逆運動学計算により関節角差分Δθを制御周期毎に順次算出する。ここで、誤差Δｘと関節角差分Δθには、ヤコビ行列をＪとして、式（１）が成立している。

Δｘ＝ＪΔθ …（１）

よって、ヤコビ行列Ｊの逆行列Ｊ^-1を求め、式（２）のように逆行列Ｊ^-1に誤差Δｘにかけることで、関節角差分Δθが算出される。

Δθ＝Ｊ^-1Δｘ …（２）

角度指令値算出部１０７は、関節角差分算出部１０６により算出された関節角差分を積分することにより関節角度指令値を制御周期毎に順次算出する。

駆動部１０１は、角度指令値算出部１０７により算出された関節角度指令値を制御目標値として、アクチュエータ１２０を制御周期毎に順次駆動させる。

関節角速度予測部１１５は、速度算出部１０８、第１の加速度算出部１０９、第２の加速度算出部１１０、加速度予測部１１１及び速度予測部１１２を備える。

速度算出部１０８は、検出部１０２により検出された関節角度の時間差分をとり、関節角速度を制御周期毎に順次算出する。

加速度算出部１０９は、速度算出部１０８により算出された関節角速度の時間差分をとり、関節角加速度を制御周期毎に順次算出することにより、実行後の複数の制御周期における関節角加速度を算出する。実行後の複数の制御周期における関節角加速度は、加速度記憶装置２０２に順次記憶される。

加速度算出部１１０は、関節角差分算出部１０６により算出された関節角差分の時間差分をとり、現在の制御周期における関節角加速度を算出する。

加速度予測部１１１は、図２に示すように、加速度記憶装置２０２に記憶されている実行後の複数の制御周期ｔ_-1，ｔ_-2，ｔ_-3における関節角加速度

（便宜上、以下の数式を除く文中においては「θ’’」と表記する。）と、加速度算出部１１０により算出された現在の制御周期ｔ_０における関節角加速度θ’’から、補間式を用いて、実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角加速度θ’’を予測する。例えば最小二乗法により式（３）のような２次多項式の補間式が算出される。

ここで、ａ₀，ａ₁，ａ₂，は係数であり、ｔは制御周期である。補間式により、図２に実線で示すような予測加速度曲線が描かれる。

速度予測部１１２が、加速度予測部１１１により予測された実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角加速度θ’’を積分することにより、図３の予測速度曲線（実線）上に示すように実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角速度

（便宜上、以下の数式を除く文中においては「θ’」と表記する。）をそれぞれ予測する。

速度判定部１１３が、速度予測部１１２により予測された実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角速度θ’が、最大速度記憶装置２０３に記憶されている最大速度（速度閾値）θ’maxを越えるか判定する。最大速度記憶装置２０３に記憶されている最大速度θ’maxは、ロボットを構成する部品の種類等により適宜設定可能である。例えば、図３において、実行前の制御周期ｔ_４における関節角速度θ’が最大速度θ’maxを越えると判定される。

抑制速度算出部１１４が、最大速度θ’maxを越えると判定された実行前の制御周期ｔ_４における関節角速度θ’が、式（４）のように最大速度θ’max以下となるように抑制する抑制関節角速度θ’を算出する。

ここで、実行前の制御周期ｔ_４における関節角速度θ’が最大速度θ’max以下であっても、実行前の制御周期ｔ_４における関節角加速度がある程度大きければ実行前の制御周期ｔ_４以降に最大速度θ’maxを超えてしまう場合がある。したがって、実行前の制御周期ｔ_４において、式（５）のように関節角加速度θ’’が０、又は式（６）のように実行前の制御周期ｔ_４までの制御周期とは反対符号となるように、図２の設計加速度曲線（破線）上に示すような抑制関節角加速度θ’’を設定する。

ただし、現在の制御周期（現在時刻）を０としている。

更に、抑制速度算出部１１４が、現在の制御周期ｔ₀から時刻ｔ_４までの間をロボットの移動が滑らかに遷移するように、例えば線形補間を行い、図２の設計加速度曲線（破線）上に示すような実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３における抑制関節角加速度θ’’を設定する。更に、抑制速度算出部１１４が、設定された実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における抑制関節角加速度θ’’を積分することにより、図３の設計加速度曲線（破線）上に示すように実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における抑制関節角速度θ’を算出する。

位置指令値算出部１０４は、抑制速度算出部１１４により算出された実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における抑制関節角速度θ’に基づいて、実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における先端位置指令値を算出する。

リンクパラメータ記憶装置２００は、位置算出部１０３によりロボットの先端位置を算出する際に使用するロボットの複数のリンクに関するリンクパラメータを記憶している。位置データ記憶装置２０１は、位置指令値算出部１０４により先端位置指令値を算出する際に使用する目標先端位置データを記憶している。加速度記憶装置２０２は、第１の加速度算出部１０９により算出された実行後の複数の制御周期ｔ_-1，ｔ_-2，ｔ_-3における関節角加速度θ’’を順次記憶し、更に加速度予測部１１１により予測された現在の制御周期の関節角加速度θ’’等を記憶している。最大速度記憶装置２０３は、速度判定部１１３による判定を行う際の閾値となる最大速度θ’maxを予め記憶している。

リンクパラメータ記憶装置２００、位置データ記憶装置２０１、加速度記憶装置２０２、及び最大速度記憶装置２０３としては、例えばメモリ、磁気ディスク、光ディスク等が採用可能である。メモリは、ＣＰＵ１００において実行されるプログラムを格納しているプログラム記憶装置等として機能させることが可能である（プログラムの詳細は後述する。）。メモリは、ＣＰＵ１００におけるプログラム実行処理中に利用されるデータ等を一時的に格納したり、作業領域として利用される一時的なデータメモリ等として機能させることも可能である。

次に、本発明の実施の形態に係るロボット制御方法の一例を、図４のフローチャートを参照しながら説明する。

（イ）ステップＳ１０１において制御計算を開始する。ステップＳ１０２において、位置指令値算出部１０４が、位置データ記憶装置２０１に記憶されている目標先端位置データ列から、先端位置指令値を制御周期毎に順次算出する。ステップＳ１０３において、位置算出部１０３が、検出部１０２により検出された関節角度からロボットの先端位置を制御周期毎に順次算出する。誤差算出部１０５が、位置算出部１０３により算出された先端位置と、位置指令値算出部１０４により算出された先端位置指令値との誤差を制御周期毎に順次算出する。ステップＳ１０４において、関節角差分算出部１０６が、式（２）を用いた逆運動学計算により、関節角差分を制御周期毎に順次算出する。

（ロ）ステップＳ１０５において、図５のステップＳ２０１〜Ｓ２０７により関節角加速度予測処理を行う。ステップＳ２０１において関節角速度予測処理を開始する。ステップＳ２０２において、検出部１０２が、関節１２１の関節角度を制御周期毎に順次検出する。ステップＳ２０３において、速度算出部１０８が、検出部１０２により検出された関節角差分の時間差分をとり、関節角速度を制御周期毎に順次算出する。ステップＳ２０４において、第１の加速度算出部１０９が、速度算出部１０８により算出された関節角速度の時間差分をとり、関節角加速度を制御周期毎に順次算出することにより図２に示すように実行後の複数の制御周期ｔ_-1，ｔ_-2，ｔ_-3における関節角加速度θ’’を算出する。実行後の複数の制御周期ｔ_-1，ｔ_-2，ｔ_-3における関節角加速度θ’’は、加速度記憶装置２０２に順次記憶されている。また、加速度算出部１１０は、関節角差分算出部１０６により算出された関節角差分の時間差分をとり、現在の制御周期ｔ_０における関節角加速度を算出する。ステップＳ４０５において、加速度予測部１１１が、加速度記憶装置２０２に記憶されている実行後の複数の制御周期ｔ_-1，ｔ_-2，ｔ_-3における関節角加速度θ’’、及び加速度算出部１１０により算出された現在の制御周期ｔ_０における関節角加速度θ’’から、最小二乗法等により式（３）のような補間式を算出し、補間式を用いて、実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角加速度θ’’を予測する。ステップＳ２０６において、速度予測部１１２が、加速度予測部１１１により予測された実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角加速度θ’’を積分することにより、図３に示すように実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角速度θ’を予測する。ステップＳ２０７において処理を終了する。

（ハ）図４のステップＳ１０６において、速度判定部１１３が、速度予測部１１２により予測された実行後の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角速度θ’が、最大速度記憶装置２０３に記憶されている最大速度θ’maxを超えているか判定する。実行後の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角速度θ’の少なくともいずれかがが最大速度θ’maxを超えていればＳ１０７に進む。

（ニ）ステップＳ１０７において、図６のステップＳ３０１〜Ｓ３０４により抑制速度算出部１１４が抑制関節角速度算出処理を行う。ステップＳ３０１において、抑制関節角速度算出処理を開始し、Ｓ３０２において、最大速度θ’maxを越えると判定された実行前の制御周期ｔ_４における関節角速度θ’が、式（４）のように最大速度θ’max以下となるように抑制する抑制関節角速度θ’を算出する。ここで、加速度予測部１１１により予測された実行後の制御周期ｔ_４における関節角加速度θ’’に基づいて、関節角速度θ’が最大速度θ’maxを超えると判定された制御周期ｔ_４において式（５）のように関節角加速度θ’’が０、又は式（６）のように制御周期ｔ_４までの関節角加速度θ’’とは反対符号となるように抑制関節角加速度θ’’を設定する。更に、現在の制御周期ｔ_０から制御周期ｔ_４までの間をロボットが滑らかに遷移できるように、例えば線形補間により実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３における抑制関節角加速度θ’’を設定する。ステップＳ３０３において、抑制速度算出部１１４が、実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角加速度θ’’を積分することにより、図２に示すように実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における抑制関節角速度θ’を算出する。ステップＳ３０４において、抑制関節角速度算出処理を終了する。その後、ステップＳ１０２に戻り、位置指令値算出部１０４が、抑制関節角速度θ’に基づいて実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における先端位置指令値を算出する。

（ホ）ステップＳ１０７において、加速度予測部１１１により予測された範囲で実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角速度θ’が最大速度θ’maxを超えない場合には、ステップＳ１０８において位置指令値算出部１０４が位置データ記憶装置２０１に記憶されている目標先端位置データ列から算出した関節角度指令値をそのまま用いて関節角度指令値を制御目標値として関節を駆動させる。ステップＳ１０９において制御計算の終了を確認する。制御計算が終了であればステップＳ１１０において処理を完了し、一方、制御計算が終了でなければ、ステップＳ１０３において再び先端位置指令値を算出する。

本発明の第１の実施の形態によれば、アクチュエータ１２０の関節角度から、実行後の複数の制御周期ｔ_-1，ｔ_-2，ｔ_-3における関節角加速度θ’’を算出し、実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角速度θ’を予測し、抑制関節角速度θ’を算出することにより、実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４において最大速度θ’maxを超えないように動作させることができる。よって、特異点近傍のような関節角速度θ’が急変するような領域をロボットが通過するような動作であっても安定して動作させることができる。

更に、予測した実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角速度θ’から線形補間等により実行前の複数の制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における抑制関節角速度θ’を算出することにより、特異点近傍の前後においても速度を滑らかに遷移させて動作させることができる。

なお、図４、図５、図６及び図８に示した一連の手順、例えば：関節１２１の関節角度を制御周期毎に検出部１０２に検出させるステップ；関節角度から、ロボットの先端位置を制御周期毎に算出するステップ；先端位置を制御するための先端位置指令値を制御周期毎に算出するステップ；先端位置と先端位置指令値との誤差を制御周期毎に算出するステップ；誤差から逆運動学計算により関節角差分を制御周期毎に算出するステップ；関節角差分を積分することにより関節角度指令値を制御周期毎に算出するステップ；関節角度指令値に基づいてアクチュエータ１２０を制御周期毎に駆動させるステップ；関節角度から関節角加速度を制御周期毎に算出することにより実行後の複数の制御周期における関節角加速度を算出し、実行後の複数の制御周期における関節角加速度から、実行前の制御周期における関節角速度を予測するステップ；実行前の制御周期における関節角速度が、最大速度記憶装置に記憶されている最大速度を超えるか判定するステップ；実行前の制御周期における関節角速度が最大速度を超えると判定された場合、実行前の制御周期における関節角速度から、実行前の制御周期における抑制関節角速度をそれぞれ算出するステップ等は、図４、図５、図６及び図８と等価なアルゴリズムのプログラムにより、図１に示したロボット制御装置を制御して実行できる。このプログラムは、例えば位置データ記憶装置２０１等に記憶させればよい。又、このプログラムは、コンピュータ読取り可能な記録媒体に保存し、この記録媒体を位置データ記憶装置２０１等に読み込ませることにより、本発明の実施の形態の一連の手順を実行することができる。更に、インターネット等の情報処理ネットワークを介して、位置データ記憶装置２０１等に格納することが可能である。

（第２の実施の形態）
本発明の第１の実施の形態では、アクチュエータ１２０の関節角度から関節角速度、関節角加速度を算出することで実行前の複数の制御周期における関節角速度を予測する場合について説明したが、本発明の第２の実施の形態では、先端位置指令値から実行前の複数の制御周期における関節角速度を予測する場合について説明する。

本発明の第２の実施の形態に係るロボット制御装置は、図７に示すように、中央演算処理装置（ＣＰＵ）１００ｘ、駆動部１０１、検出部１０２、アクチュエータ１２０、リンクパラメータ記憶装置２００、位置データ記憶装置２０１、加速度記憶装置２０２、及び最大速度記憶装置２０３を備える。

ＣＰＵ１００ｘは、位置算出部１０３、位置指令値算出部１０４、誤差算出部１０５、第１の関節角差分算出部１０６、角度指令値算出部１０７、関節角速度予測部３００、加速度予測部３０４、速度判定部１１３、抑制速度算出部１１４をハードウェア資源であるモジュール（論理回路）として論理的に備える。

関節角速度予測部３００は、先端速度算出部３０１、第２の関節角差分算出部３０２及び速度予測部３０３を備える。

先端速度算出部３０１は、位置指令値算出部１０４の計算を先回りして行い、位置指令値算出部１０４により算出された実行前の複数の制御周期における先端位置指令値に基づいて、前後２つの実行前の制御周期における先端位置指令値の差分をとり、実行前の複数の制御周期における先端速度をそれぞれ算出する。

第２の関節角差分算出部３０２は、先端速度算出部３０１により算出された実行前の複数の制御周期における先端速度から、逆運動学計算により実行前の複数の制御周期における関節角差分をそれぞれ算出する。

速度予測部３０３は、第２の関節角差分算出部３０２により算出された実行前の複数の制御周期における関節角差分から、例えば最小二乗法により式（３）のような補間式を用いて、実行前の複数の制御周期における関節角速度を予測する。先端位置指令値は予め加減速を含んだ滑らかな軌道を生成しているので、この予測された関節角速度は理想的な速度波形となり、精度よく予測できる。

加速度予測部３０４は、速度予測部３０３により予測された実行前の複数の制御周期における関節角速度の時間差分をとり、実行前の複数の制御周期における関節角加速度をそれぞれ予測する。

角度指令値算出部１０７は、第１の関節角差分算出部１０６により算出された実行前の複数の制御周期における関節角差分に、速度予測部３０３により予測された関節角速度にフィードフォワードとしてあるゲインをかけて加え、実行前の複数の制御周期における関節角度指令値をそれぞれ算出する。この結果、制御応答性を更に向上させることができる。

図７に示した本発明の第２の実施の形態に係るロボット制御装置の他の構成は、図１に示した本発明の第１の実施の形態のロボット制御装置の構成と実質的に同様であるので、重複した説明を省略する。

次に、本発明の第２の実施の形態に係るロボット制御方法の一例を、図８のフローチャートを参照しながら説明する。

（イ）ステップＳ４０１において制御計算を開始する。ステップＳ４０２において、位置指令値算出部１０４が、位置データ記憶装置２０１に記憶されている目標先端位置データ列から、実行前の複数の制御周期の先端位置指令値を算出する。ステップＳ４０３において、検出部１０２が、実行後の複数の制御周期における関節角度を検出する。誤差算出部１０５が、位置算出部１０３により算出された先端位置と、位置指令値算出部１０４により算出された先端位置指令値との誤差を算出する。ステップＳ４０４において、関節角差分算出部１０６が、式（２）を用いた逆運動学計算により、関節角差分を算出する。ステップＳ４０５において関節角差分から関節角度指令値を算出し、関節角度指令値を制御目標値として関節を駆動させる。ステップＳ４０７において制御計算の終了を確認し、制御計算が終了であればステップＳ４０８において処理を完了し、制御計算が終了していなければ、ステップＳ４０２に戻り再び先端位置指令値を算出する。

（ロ）ステップＳ４０９において、先端速度算出部１１６が、位置指令値算出部１０４から算出された実行前の複数の制御周期に対して、その制御周期前後の先端位置指令値から差分をとり実行前の複数の制御周期における先端速度を算出する。ステップＳ４１０において、第２の関節角差分算出部３０２が、先端速度算出部１１６により算出された実行前の複数の制御周期における先端速度から逆運動学計算により実行前の複数の制御周期における関節角差分を算出する。ステップＳ４１１において、速度予測部３０３が、第２の関節角差分算出部３０２により算出された実行前の複数の制御周期における関節角差分から、式（３）のような補間式を用いて、実行前の複数の制御周期における関節角速度を予測する。

（ハ）ステップＳ４１２において速度判定部１１３が、速度予測部３０３により予測された実行前の複数の制御周期における関節角速度が、最大速度記憶装置２０３に記憶されている最大速度を超えるか判定する。実行前の複数の制御周期における関節角速度が最大速度を越えると判定された場合、ステップＳ４１３において、本発明の第１の実施の形態の手順と同様に、抑制速度算出部１１４が、速度予測部３０３により予測された実行前の複数の制御周期における関節角速度及び加速度予測部３０４により予測された実行前の複数の制御周期における関節角加速度から、抑制関節角速度を算出する。その後、ステップＳ４０２に戻り、位置指令値算出部１０４が、位置データ記憶装置２０１に記憶されている目標先端位置データ列及び抑制関節角速度に基づいて、実行前の複数の制御周期における先端位置指令値を算出する。一方、ステップＳ４１２において実行前の複数の制御周期における関節角速度が最大速度を超えないと判定された場合、ステップＳ４０２に戻り、位置指令値算出部１０４が、位置データ記憶装置２０１に記憶されている目標先端位置データ列から、予測した範囲の実行前の複数の制御周期における先端位置指令値を順次算出する。

本発明の第２の実施の形態によれば、位置指令値算出部１０４の計算を先回りして行い、実行前の複数の制御周期における関節角速度を予測し、抑制関節角速度を算出することで、特異点近傍のような関節角速度が急変するような領域をロボットが通過するような動作であっても安定して動作させることができる。

（その他の実施の形態）
上記のように、本発明は第１及び第２の実施の形態によって記載したが、この開示の一部をなす論述及び図面はこの発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施の形態、実施例及び運用技術が明らかとなろう。

例えば、本発明の第１の実施の形態において、ＣＰＵ１００、駆動部１０１、リンクパラメータ記憶装置２００、位置データ記憶装置２０１、加速度記憶装置２０２、及び最大速度記憶装置２０３等がロボットの内部に埋め込まれて一体化されていても良い。又、ＣＰＵ１００、駆動部１０１、リンクパラメータ記憶装置２００、位置データ記憶装置２０１、加速度記憶装置２０２、及び最大速度記憶装置２０３等がロボットの外部にあり、ロボットを有線或いは無線で遠隔制御することも可能である。

又、本発明の第１の実施の形態において補間式として、２次多項式である式（３）を用いたがこれに特に限定されるものではない。補間式としてはｎ次（ｎは正の整数）の多項式を用いても良い。

又、本発明の第１の実施の形態において実行前の３つの制御周期ｔ_１，ｔ_２，ｔ_３をから実行前の４つの制御周期ｔ_１，ｔ_２，ｔ_３，ｔ_４における関節角速度を予測する場合を説明したが、関節角速度の予測に用いる実行前の制御周期のステップ数、及び予測される実行前の制御周期のステップ数は適宜設定可能であり、特に限定されるものではない。

このように、本発明はここでは記載していない様々な実施の形態等を含むことは勿論である。したがって、本発明の技術的範囲は上記の説明から妥当な特許請求の範囲に係る発明特定事項によってのみ定められるものである。

本発明の第１の実施の形態に係るロボット制御装置の一例を示すブロック図である。 本発明の第１の実施の形態に係る加速度予測処理を説明するためのグラフである。 本発明の第１の実施の形態に係る速度予測処理を説明するためのグラフである。 本発明の第１の実施の形態に係るロボット制御方法の一例を説明するためのフローチャートである。 本発明の第１の実施の形態に係る関節角速度予測処理のフローチャートである。 本発明の第１の実施の形態に係る抑制関節角速度算出処理のフローチャートである。 本発明の第２の実施の形態に係るロボット制御装置の一例を示すブロック図である。 本発明の第２の実施の形態に係るロボット制御方法の一例を説明するためのフローチャートである。

符号の説明

１００，１００ｘ…中央演算処理装置（ＣＰＵ）
１０１…駆動部
１０２…検出部
１０３…位置算出部
１０４…位置指令値算出部
１０５…誤差算出部
１０６，３０２…関節角差分算出部
１０７…角度指令値算出部
１０８…速度算出部
１０９…加速度算出部
１０９，１１０…加速度算出部
１１１，３０４…加速度予測部
１１２，３０３…速度予測部
１１３…速度判定部
１１４…抑制速度算出部
１１５，３００…関節角速度予測部
１１６…先端速度算出部
１２０…アクチュエータ
１２１…関節
２００…リンクパラメータ記憶装置
２０１…位置データ記憶装置
２０２…加速度記憶装置
２０３…最大速度記憶装置
３０１…先端速度算出部

Claims (6)

1. 関節を有するロボットを制御するロボット制御装置において、
   前記関節を制御周期毎に駆動するアクチュエータと、
   前記関節の関節角度を前記制御周期毎に検出する検出部と、
   前記関節角度から、前記ロボットの先端位置を前記制御周期毎に算出する位置算出部と、
   前記先端位置を制御するための先端位置指令値を前記制御周期毎に算出する位置指令値算出部と、
   前記先端位置と前記先端位置指令値との誤差を前記制御周期毎に算出する誤差算出部と、
   前記誤差から逆運動学計算により関節角差分を前記制御周期毎に算出する関節角差分算出部と、
   前記関節角差分を積分することにより関節角度指令値を前記制御周期毎に算出する角度指令値算出部と、
   前記関節角度指令値に基づいて前記アクチュエータを前記制御周期毎に駆動させる駆動部と、
   前記関節角度から関節角加速度を前記制御周期毎に算出することにより実行後の複数の前記制御周期における関節角加速度を算出し、前記実行後の複数の制御周期における関節角加速度から、実行前の前記制御周期における関節角速度を予測する関節角速度予測部と、
   前記実行前の制御周期における関節角速度に対する最大速度を記憶する最大速度記憶装置と、
   前記実行前の制御周期における関節角速度が前記最大速度を超えるか判定する速度判定部と、
   前記実行前の制御周期における関節角速度が前記最大速度を超えると判定された場合、前記実行前の制御周期における関節角速度から、前記実行前の制御周期における抑制関節角速度を算出する抑制速度算出部
   とを備えることを特徴とするロボット制御装置。
2. 前記関節角速度予測部が、
   前記関節角度から前記関節角速度を前記制御周期毎に算出する速度算出部と、
   前記関節角速度から、前記関節角加速度を前記制御周期毎に算出することにより前記実行後の複数の制御周期における関節角加速度を算出する第１の加速度算出部と、
   前記関節角差分から、現在の制御周期の関節角加速度を算出する第２の加速度算出部と、
   前記実行後の複数の制御周期における関節角加速度及び前記現在の制御周期における関節角加速度から、前記実行前の制御周期における関節角加速度を予測する加速度予測部と、
   前記実行前の制御周期における関節角加速度から前記実行前の制御周期における関節角速度を予測する速度予測部
   とを備えることを特徴とする請求項１に記載のロボット制御装置。
3. 関節を有するロボットを制御するロボット制御装置において、
   前記関節を制御周期毎に駆動するアクチュエータと、
   前記関節の関節角度を前記制御周期毎に検出する検出部と、
   前記関節角度から、前記ロボットの先端位置を前記制御周期毎に算出する位置算出部と、
   前記先端位置を制御するための先端位置指令値を前記制御周期毎に算出する位置指令値算出部と、
   前記先端位置と前記先端位置指令値との誤差を前記制御周期毎に算出する誤差算出部と、
   前記誤差から逆運動学計算により関節角差分を前記制御周期毎に算出する第１の関節角差分算出部と、
   前記関節角差分を積分することにより関節角度指令値を前記制御周期毎に算出する角度指令値算出部と、
   前記関節角度指令値に基づいて前記アクチュエータを前記制御周期毎に駆動させる駆動部と、
   前記位置指令値算出部により算出された前記先端位置指令値から関節角差分を順次算出することにより実行前の複数の前記制御周期における関節角差分をそれぞれ算出し、前記実行前の複数の制御周期における関節角差分から、前記実行前の複数の制御周期における関節角速度をそれぞれ予測する関節角速度予測部と、
   前記実行前の制御周期における関節角速度に対する最大速度を記憶する最大速度記憶装置と、
   前記実行前の複数の制御周期における関節角速度が前記最大速度を超えるかそれぞれ判定する速度判定部と、
   前記実行前の制御周期における関節角速度が前記最大速度を超えると判定された場合、前記最大速度を超えると判定された前記実行前の制御周期における関節角速度から、前記最大速度を超えると判定された前記実行前の制御周期における抑制関節角速度を算出する抑制速度算出部
   とを備えることを特徴とするロボット制御装置。
4. 前記関節角速度予測部が、
   前記先端位置指令値から、前記実行前の複数の制御周期における先端速度をそれぞれ算出する先端速度算出部と、
   前記実行前の複数の制御周期における先端速度から、前記実行前の複数の制御周期における関節角差分をそれぞれ算出する第２の関節角差分算出部と、
   前記実行前の複数の制御周期における関節角差分から、前記実行前の複数の制御周期における関節角速度をそれぞれ予測する速度予測部
   とを備えることを特徴とする請求項３に記載のロボット制御装置。
5. 前記抑制速度算出部が、
   前記最大速度を超えると判定された前記実行前の制御周期における関節角加速度を、０又は前記最大速度を超えると判定されるまでの制御周期とは反対符号に設定し、
   前記設定された関節角加速度に基づいて、前記最大速度を超えると判定されるまでの制御周期における関節角加速度を設定し、
   設定された前記最大速度を超えると判定された前記実行前の制御周期における関節角加速度、及び設定された前記最大速度を超えると判定されるまでの制御周期における関節角加速度をそれぞれ積分することにより、前記抑制関節角速度を算出する
   ことを特徴とする請求項１〜４のいずれか１項に記載のロボット制御装置。
6. 前記角度指令値算出部が、前記実行前の制御周期における前記関節角差分にゲインをかけ、前記実行前の制御周期における前記関節角度指令値に加えることを特徴とする請求項４に記載のロボット制御装置。

Images