JP6915470B2 - ロボットの制御装置 - Google Patents

Description

本発明は、ロボットの動作を制御する装置に関する。

従来、ロボットのアームの先端近傍に加速度量を検出する加速度センサを取り付け、加速度センサにより検出された加速度量に基づいて、アームの先端に生じた振動を抑制するように各関節部の駆動部を制御する指令値を補償するものがある（特許文献１参照）。

特開平１０−１０００８５号公報

ところで、このように加速度センサにより検出された加速度量を用いる手法では、実際に振動が発生するアームの先端近傍に加速度センサを取り付ける必要がある。しかしながら、現実のロボットに適用する場合には、アームの先端近傍に加速度センサを取り付けるスペースや、加速度センサまでの配線の取り回し等が問題となり、実現が困難である。

本発明は、上記事情に鑑みてなされたものであり、その目的は、加速度センサを用いることなくロボットの振動を抑制できるロボットの制御装置を提供することにある。

請求項１記載のロボットの制御装置によれば、設置面に対し垂直となる第１軸と、この第１軸に対して垂直となる第２軸と、この第２軸以降に連結される複数の軸のうち、少なくとも１つが第２軸に対して平行となる軸を備える４軸以上のアームを有するロボットを制御対象とする。速度パターン生成部は、アームの手先を移動させる最終目標位置が与えられると、その手先を最終目標位置に移動させる速度パターンを生成する。指令値算出部は、手先を制御周期毎の通過目標位置に移動させるように、サーボ機構に与える指令値を算出する。

上記構成のアームを有するロボットについては、アーム全体を１つのバネと見做し、
「アームのイナーシャ」−「アームバネ」−「ワークのイナーシャ」が連結したモデルを想定することができる。この場合、「アームのイナーシャ」と「アームバネ」とは、手先の位置に応じて変化することになる。そして、アームが手先に保持したワークを移動させるため加速する際には、慣性力によってアームの先端側の移動が遅れ、減速する際には、逆に先端側の移動が先に進む。これにより、振動が発生する。

そこで、バネ定数演算部は、手先の現在位置に応じてアームのバネ定数を演算し、イナーシャ演算は、同じく手先の現在位置に応じて、アームと手先に保持されるワークとについてのイナーシャを演算する。そして、加速度算出部が、指令値が制御周期毎に変化する際の加速度を算出する。補正量演算部は、バネ定数，イナーシャ及び加速度から指令値の補正量を演算すると、指令値補正部は、指令値を補正量に基づき補正してサーボ機構に出力する。

これにより、アームの加速時には、先端側が遅れる分だけアームの基部側が先に進むように指令値が補正され、アームの減速時には、先端側が進む分だけアームの基部側が遅れるように指令値が補正されて、振動が抑制される。したがって、アームの先端部に加速度センサ等を取り付けずとも、アームが手先に保持したワークを移動させる際に発生する振動を抑制することができる。

請求項２記載のロボットの制御装置によれば、バネ定数演算部は、アーム全体のバネ定数を演算し、イナーシャ演算部は、アーム全体及びワークを含むイナーシャを演算する。これにより、演算量を減らして補正処理を高速に実行できる。

請求項３記載のロボットの制御装置によれば、バネ定数演算部は、第３軸から手先までの距離をＬ_３，第１軸から手先までの距離をＬ_ｆ，第１軸を基準とする第３軸までのバネ定数をＫ_１２，第３軸を基準とする手先までのバネ定数をＫ_３とすると、補正量を求めるために使用するバネ定数Ｋを、次式
１／Ｋ＝｛１／Ｋ_１２＋Ｌ_３ ^２／（Ｋ_３Ｌ_ｆ ^２）｝
で求める。これにより、アームを一体的にバネとして捉えた際のバネ定数Ｋを迅速に求めることができる。

請求項４記載のロボットの制御装置によれば、バネ定数演算部は、アームの各軸を基準とするバネ定数を演算し、イナーシャ演算部は、各軸を基準とするイナーシャを演算する。そして、補正量演算部は、各軸に対応するイナーシャを各軸に対応するバネ定数で除して各軸に対応する補正係数を求め、それらの補正係数のうち最大値を示したものを用いて各軸に適用する補正量を決定する。すなわち、アームの各軸の物理特性は、それぞれの回転方向位置によって異なる。しかし、各軸に対応する補正係数を個別に適用すると、各軸の遅れ量が大きく異なって、当初に予定した手先の移動軌跡が維持できなくなる可能性が有る。そこで、補正量演算部が、補正係数のうち最大値を示したものを用いて各軸に適用する補正量を決定することにより、移動軌跡を維持できる範囲で振動を抑制できる。

請求項５記載のロボットの制御装置によれば、補正量演算部は、バネ定数を１次近似式により演算する。例えば、請求項３のようにし演算して得られるバネ定数Ｋは、距離Ｌ_ｆの２乗に対して曲線を描く。その曲線に対して値域を複数に区切った区間については、誤差を大きくすることなく１次近似を行うことができる。したがって、補正に使用するバネ定数Ｋをより簡単に得ることができる。

請求項６記載のロボットの制御装置によれば、指令値補正部は、補正した指令値に対しローパスフィルタ処理を行う。補正値には加速度がパラメータとして含まれているため、加速度に広域のノイズ成分が重畳されたり、立上りが急峻になるとトルクが飽和する場合がある。そこで、補正した指令値についてローパスフィルタ処理を行うことにより、トルクが飽和することを防止できる。

請求項７記載のロボットの制御装置によれば、指令値補正部は、イナーシャをバネ定数で除した補正係数に応じて、ローパスフィルタの遮断周波数を可変設定する。これにより、イナーシャ及びバネ定数も考慮して遮断周波数を適切に設定できる。

第１実施形態であり、ロボットシステムにおけるコントローラの構成を中心に示す機能ブロック図 ロボットシステムの構成を示す図 制振制御に用いるロボットのモデルを説明する図 本実施形態を適用対象とするロボットの構成を説明する図（その１） 本実施形態を適用対象とするロボットの構成を説明する図（その２） 本実施形態を適用対象とするロボットの構成を説明する図（その３） 制振制御の原理を説明する図 バネ定数Ｋを求める式の導出を説明する図 制振制御の内容を示すフローチャート 比較例及び本実施形態の制振制御を適用した角速度パターンを示すタイムチャート 比較例及び本実施形態における目標位置と実際の手先位置との誤差を示すタイムチャート 第２実施形態であり、制振制御の内容を示すフローチャート 第３実施形態であり、バネ定数Ｋを（７）式で求めた曲線を示す図 図１３に示すバネ定数Ｋを一次近似した一例を示す図

（第１実施形態）
以下、第１実施形態について図１から図１１を参照しながら説明する。図２に示すように、ロボットシステム１は、垂直多関節型のロボット２、ロボット２を制御するコントローラ３、コントローラ３に接続されたペンダント４を備えている。このロボットシステム１は、一般的な産業用に用いられている。

ロボット２は、いわゆる６軸の垂直多関節型ロボットとして周知の構成を備えており、ベース５上に、Ｚ方向の軸心を持つ第１軸（Ｊ１）を介してショルダ６が水平方向に回転可能に連結されている。ショルダ６には、Ｙ方向の軸心を持つ第２軸（Ｊ２）を介して上方に延びる下アーム７の下端部が垂直方向に回転可能に連結されている。下アーム７の先端部には、Ｙ方向の軸心を持つ第３軸（Ｊ３）を介して第一上アーム８が垂直方向に回転可能に連結されている。

第一上アーム８の先端部には、Ｘ方向の軸心を持つ第４軸（Ｊ４）を介して第二上アーム９が捻り回転可能に連結されている。第二上アーム９の先端部には、Ｙ方向の軸心を持つ第５軸（Ｊ５）を介して手首１０が垂直方向に回転可能に連結されている。手首１０には、Ｘ方向の軸心を持つ第６軸（Ｊ６）を介してフランジ１１が捻り回転可能に連結されている。

ベース５、ショルダ６、下アーム７、第一上アーム８、第二上アーム９、手首１０およびフランジ１１は、ロボット２のアームとして機能し、アームの先端となるフランジ１１には、図１に示すようにワーク１３を保持するための図示しないツールが取り付けられる。ロボット２に設けられている各軸（Ｊ１〜Ｊ６）には、それぞれに対応して駆動源となる図示しないモータが設けられている。

コントローラ３は、ロボット２の制御装置であり、図示しないＣＰＵ、ＲＯＭおよびＲＡＭ等で構成されたコンピュータからなる制御手段においてコンピュータプログラムを実行することで、ロボット２を制御している。具体的には、コントローラ３は、インバータ回路等から構成された駆動部を備えており、各モータに対応して設けられているエンコーダで検知したモータの回転位置に基づいて例えばフィードバック制御によりそれぞれのモータを駆動する。

コントローラ３は、ＣＰＵ、ＲＯＭ、ＲＡＭ、駆動回路、及び位置検出回路等を備えている。ＲＯＭは、ロボット２のシステムプログラムや動作プログラム等を記憶している。ＲＡＭは、これらのプログラムを実行する際にパラメータの値等を記憶する。位置検出回路には、ロボット２の各関節に設けられた図示しない各エンコーダの検出信号がそれぞれ入力される。位置検出回路は、各エンコーダの検出信号に基づいて、各関節に設けられたモータの回転角度位置を検出する。図１に示すように、こうしたコントローラ３において、軌道生成部２１、指令値補正部２２及びサーボ機構２３が構成されている。

コントローラ３は、ロボット２の動作プログラム等に基づいて、第６軸の先端部である手先の目標停止位置を算出する。そして、コントローラ３は、目標停止位置に基づいて、各軸の目標停止角度位置を算出する。軌道生成部２１は、各軸の目標停止角度位置に基づいて、各軸の角度位置を目標停止角度位置まで変更する際の各軸の角速度パターンを生成する。軌道生成部２１は、各軸の角速度パターンに基づいて、各軸の直近の目標角度位置である指令値θｎを算出する。駆動制御部に相当するサーボ機構２３は、各エンコーダにより検出された各軸の角度θｅｎと指令値θｎとに基づいて、各軸の角度を指令値θｎとするように、各モータを各駆動トルクＴｎで制御する。指令値補正部２２は、指令値θｎに補正を加えた指令値θｎ’をサーボ機構２３に出力する。その補正の詳細については後述する。

本実施形態では、コントローラ３は、ロボット２を図３に示す動作モデルとして捉え、手先の振動を抑制する制振制御を実行する。同図に示すように、６軸のロボット２は、基部にある第１軸のモータに対して６軸のアームが連結されており、そのアームの先端にワーク１３が保持されている。この場合、６軸のアームを１つのバネとして見ると、そのバネにアームのイナーシャと、ワーク１３である負荷のイナーシャとが連結されたモデルとして見ることができる。

次に、このモデルが適用可能なロボットのアーム構造について図４から図６を参照して説明する。図４は、図２に示す垂直６軸ロボットに対応したもので、設置面に対し垂直となる第１軸と、この第１軸に対して垂直となる第２軸と、この第２軸以降に連結される複数の軸のうち、少なくとも１つが第２軸に対して平行となる第３軸を備える４軸以上のアームを有するロボットである。図５は、図４に示す構成の第５軸を除いて、第６軸に相当するものを第５軸とした垂直５軸ロボットである。また、図６は、図４に示す構成の第２軸と第３軸との間に、第２軸に直行する第７軸を加えた垂直７軸ロボットである。これらに対して、図２に示すモデルが同様に適用できる。

図７は、本実施形態の制振制御の原理を説明するもので、本実施形態における制御と比較例とを示す。図中における上方のポイントがアームの基部側，第１軸のモータ側に、下方のポイントが先端側，負荷側に相当し、アームを１つのリンクのみに単純化している。このモデルにより、基部側が図中右方向にリニアに移動する場合について説明する。上段の図が、基部側を単に移動させる場合の比較例であり、下段の図が本実施形態の制御原理を適用した場合である。

比較例では、基部が停止状態から移動を開始して右方向に加速すると、先端部には慣性力が作用するため、その分だけ移動が遅れる。逆に、基部が移動状態から停止させるために減速すると、先端部には慣性力が作用する分だけ移動位置が進む。そして、基部が停止すると、先端部の位置は、先に進んだ位置から左方向に戻る。このような動きが、アームに振動を発生させている。

この比較例の動きに対して本実施形態の制御原理を適用すると、基部が加速する際には、先端部の移動が遅れる分だけ基部を先行させるように加速させる。また、基部が減速する際には、先端部の位置が進んでいる分だけ基部を遅らせるように減速させる。そして、基部を停止させる際には、先端部が左方向に戻ろうとする復元力に合せて基部を移動させる。このように制御することで、アームの振動を抑制する。

次に、実際の制御に使用するバネ定数Ｋの求め方について、図８を参照して説明する。図中右側のポイントが、６軸ロボット２の第１軸及び第２軸（Ｊ１，Ｊ２）の位置を示し、図中左側のポイントが手先の位置を示し、それらの間にあるポイントが第３軸（Ｊ３）の位置を示す。

アームを横から見た状態のＸＺ平面内において、第１軸及び第２軸から第３軸までの距離をＬ_１，第３軸から手先までの距離をＬ_３，第１軸及び第２軸から手先までの距離をＬ_ｆとする。また、アームを上から見た状態のＸＹ平面内において、Ｘ軸を基準とする第３軸のＹ軸方向への角度偏差をΔθ_１２とし、Ｘ軸を基準とする手先のＹ軸方向への角度偏差をΔθ_３とする。

手先に対し、質量Ｍ，加速度ａによる慣性力Ｆ（＝Ｍ・ａ）が作用した際に、第１軸及び第２軸作用するトルクτ_１２，第３軸に作用するトルクτ_３は、それぞれ
τ_１２＝Ｌ_ｆ×Ｆ，τ_３＝Ｌ_３×Ｆ …（１）
となる。

第１軸及び第２軸を基準とする手先のバネ係数をＫ_１２とし、第３軸を基準とする手先のバネ係数をＫ_３とすると、角度偏差Δθ_１２，Δθ_３は、
Δθ_１２＝τ_１２／Ｋ_１２，Δθ_３＝τ_３／Ｋ_３ …（２）
となり、Ｙ軸方向への位置変位ΔＹ_１２，ΔＹ_３は、
ΔＹ_１２＝Ｌ_ｆ×τ_１２／Ｋ_１２＝Ｌ_ｆ ^２×Ｆ／Ｋ_１２
ΔＹ_３＝Ｌ_３×τ_３／Ｋ_３＝Ｌ_３ ^２×Ｆ／Ｋ_３ …（３）
となる。

位置変位の合計ΔＹ_ｆは、
ΔＹ_ｆ＝ΔＹ_１２＋ΔＹ_３＝Ｌ_ｆ ^２×Ｆ／Ｋ_１２＋Ｌ_３ ^２×Ｆ／Ｋ_３
＝（Ｌ_ｆ ^２／Ｋ_１２＋Ｌ_３ ^２／Ｋ_３）Ｆ …（４）
である。また（１）式より、Ｆ＝τ_１２／Ｌ_ｆであるから、
ΔＹ_ｆ＝（Ｌ_ｆ ^２／Ｋ_１２＋Ｌ_３ ^２／Ｋ_３）τ_１２／Ｌ_ｆ …（５）
となる。

角度偏差の合計Δθ_ｆは、ここでは、サンプリング周期毎の角度偏差であり、
Δθ_ｆ≒０であるから、
Δθ_ｆ≒ｔａｎΔθ_ｆ＝ΔＹ_ｆ／Ｌ_ｆ
＝（Ｌ_ｆ ^２／Ｋ_１２＋Ｌ_３ ^２／Ｋ_３）τ_１２／Ｌ_ｆ ^２
＝（１／Ｋ_１２＋１／Ｋ_３×Ｌ_３ ^２／Ｌ_ｆ ^２）τ_１２ …（６）
となる。そして、（６）式の括弧内部は、バネ定数Ｋの逆数であるから、
１／Ｋ＝（１／Ｋ_１２＋１／Ｋ_３×Ｌ_３ ^２／Ｌ_ｆ ^２） …（７）
である。このようにバネ定数Ｋは、各軸の剛性，つまりバネ定数Ｋ_１２，Ｋ_３及び手先距離Ｌ_ｆの逆数に依存した関数となる。バネ定数Ｋ_１２，Ｋ_３については、第２軸，第３軸に使用されているクロスローラベアリングの剛性値を用いるか、予め実測した値を用いる。本実施形態では、この（７）式で示されるバネ定数Ｋを制振制御に使用する。

図９は、本実施形態における制振制御の手順を示すフローチャートである。軌道生成部２１は、各軸の目標位置である手先の停止時の角度位置を取得すると（Ｓ１）、その各軸の目標位置に基づいて各軸の角速度パターンを生成する（Ｓ２）。例えば、後述する図１０に破線で示す指令角速度のように、台形の角速度パターンを生成する。尚、各図中に示す「速度」は何れも「角速度」である。また、ステップＳ１における目標位置は、最終目標位置に相当する。

続いて、軌道生成部２１は、生成した角速度パターンに基づいて、指令値である各軸の目標角度θ（ｎ＝１〜６）を算出する（Ｓ３）。ここでの目標角度θｎは、今回の制御周期，つまり各軸エンコーダが検出した角度θｅｎをサンプリングする周期における第ｎ軸の目標角度に相当する。また、軌道生成部２１は、速度パターン生成部及び指令値生成部に相当する。

次に、指令値補正部２２は、各軸の目標角度から手先の目標位置Ｐを求めると（Ｓ４）、その目標位置Ｐに応じたバネ定数Ｋを（７）式で求めると共に、イナーシャＩを求める（Ｓ５）。尚、目標位置Ｐを求める際に、（７）式の演算パラメータである距離Ｌ_３及びＬ_ｆも求められる。イナーシャＩは、ワーク１３及びアームの質量と距離Ｌ_３との積である。目標位置Ｐは通過目標位置に相当する。続いて、各軸の速度パターンから、各軸がそれぞれ目標角度に至る間の角加速度ω（ドット）を計算する（Ｓ６）。

次に、指令値補正部２２は、角度の補正量｛Ｉω（ドット）／Ｋ｝を計算する（Ｓ７）。すなわち、Δθ＝Ｔ／Ｋ＝Ｉ・ｄω／ｄｔ／Ｋである。そして、各軸の角度指令値θｎに補正量｛Ｉω（ドット）／Ｋ｝を加えた修正指令値θｎ’を計算すると（Ｓ８）、その修正指令値θｎ’にローパスフィルタ処理を行ってから（Ｓ９）サーボ機構２３に転送する（Ｓ１０）。それから、ステップＳ３に戻る。指令値補正部２２は、バネ定数演算部，イナーシャ演算部，加速度算出部及び補正量演算部に相当する。

尚、ステップＳ９においてローパスフィルタ処理を行うのは、以下の理由による。補正量に角加速度ω（ドット）がパラメータとして含まれているため、角加速度に広域のノイズ成分が重畳されたり、立上りが急峻になるとトルクが飽和する場合がある。そこで、補正した指令値についてローパスフィルタ処理を行うことにより、トルクが飽和することを防止する。その遮断周波数は、例えば数Ｈｚ〜１０数Ｈｚ程度である。

図１０は、比較例である通常制御及び本実施形態による角速度パターンを示すタイムチャートである。破線は、ステップＳ２において生成された角速度パターンに相当し、指令角速度を示している。一点鎖線は、生成された角速度パターンに基づきステップＳ３で算出した指令値θを、そのままサーボ機構２３に転送した場合の角速度を示している。実線は、本実施形態の制振制御を実行した場合の角速度を示している。

同図に示すように、停止状態からの加速開始時に、本実施形態の制御では比較例よりも角速度が上昇するタイミングが早くなっている。これは、図７に示す（２）加速時の動きに対応する。またこのとき、ステップＳ９において修正指令値θ’がローパスフィルタ処理されているため、修正指令値θ’の急激な変化が抑制されている。これにより、修正指令値θ’に基づいて各軸モータが制御された際に、各軸の角速度が急激に変化することが抑制されている。

また、加速時から等速時への移行時に、本実施形態の制御では比較例よりも角速度が一定になるタイミングが遅くなっている。これは、図７に示す（３）減速時の動きに対応する。

次の等速時から減速時への移行時においても、本実施形態の制御では比較例よりも角速度の低下するタイミングが早くなり、次の減速時から移動終了時への移行時においても、本実施形態制御では比較例よりも角速度が低下するタイミングが遅くなっている。これは、図７に示す（３）減速時及び（４）停止時の動きに対応する。

図１１は、上記の移動終了時における比較例及び本実施形態における目標位置と実際の手先位置との誤差をシミュレーションしたタイムチャートである。同図に一点鎖線で示すように、比較例では、目標位置との誤差が負側と正側とに交互に変化しており、アームの手先が振動している。これに対して、実線で示すように、本実施形態の制御では、目標位置に対する誤差が０に近くなっている。すなわち、手先の振動が抑制されている。

以上のように本実施形態によれば、軌道生成部２１は、アームの手先を移動させる最終目標位置が与えられると、その手先を最終目標位置に移動させる速度パターンを生成し、手先をサンプリング周期毎の目標位置に移動させるように、サーボ機構２３に与える角度指令値θｎを算出する。指令値補正部２２は、手先の現在位置に応じてアームのバネ定数Ｋを演算し、アームと手先に保持されるワークとについてのイナーシャＩを演算し、指令値がサンプリング周期毎に変化する際の加速度ω（ドット）を算出する。そして、これらの値から指令値の補正量｛Ｉω（ドット）／Ｋ｝を演算すると、指令値θｎを前記補正量に基づき補正してサーボ機構２３に出力する。

これにより、アームの加速時には、先端側が遅れる分だけアームの基部側が先に進むように指令値が補正され、アームの減速時には、先端側が進む分だけアームの基部側が遅れるように指令値が補正されて、振動が抑制される。したがって、アームの先端部に加速度センサ等を取り付けずとも、アームが手先に保持したワーク１３を移動させる際に発生する振動を抑制することができる。

また、指令値補正部２２は、アーム全体のバネ定数Ｋを演算し、アーム全体及びワーク１３を含むイナーシャＩを演算するので、演算量を減らして補正処理を高速に実行できる。具体的には、バネ定数Ｋを（７）式で求めるので、アームを一体的にバネとして捉えた際のバネ定数Ｋを迅速に求めることができる。これにより、ロボット２を高速で動作させることができる。

また、指令値補正部２２は、補正した指令値θｎ’に対しローパスフィルタ処理を行う。これにより、加速度に広域のノイズ成分が重畳されたり、立上りが急峻になる場合でも、トルクが飽和することを防止できる。

（第２実施形態）
以下、第１実施形態と同一部分には同一符号を付して説明を省略し、異なる部分について説明する。図１２に示す第２実施形態の制御では、ステップＳ４を実行すると、指令値補正部２２は、目標位置Ｐから、各軸のバネ定数Ｋ１〜Ｋ６と、イナーシャＩ１〜Ｉ６とを求める（Ｓ１１）。バネ定数Ｋ１〜Ｋ６については、各軸に（７）式を適用することで計算する。更に、それらに基づき、各軸の補正係数Ｉ１／Ｋ１〜Ｉ６／Ｋ６を求めると（Ｓ１２）、それらのうち値が最大となる補正係数ｍａｘ（Ｉ／Ｋ）をＭａｘＡとして選択する（Ｓ１３）。続いて、ステップＳ６を実行すると、補正係数ＭａｘＡを用いて補正量Ａｄｊ｛＝ＭａｘＡ・ω（ドット）｝を計算し（Ｓ１４）、指令値θに補正量Ａｄｊを加えて修正する（Ｓ１５）。

すなわち、アームの各軸の物理特性は、それぞれの回転方向位置によって異なる。しかし、各軸に対応する補正係数を個別に適用すると各軸の遅れ量が大きく異なり、当初に予定した手先の移動軌跡が維持できなくなる可能性が有る。そこで、補正係数の最大値ＭａｘＡを用いて各軸に適用する補正量Ａｄｊを決定することで、移動軌跡を維持できる範囲で振動を抑制する。続いて、ステップＳ９と同様に修正指令値θ’に対してローパスフィルタ処理を行うが、ここでのフィルタの帯域，遮断周波数は、補正係数ＭａｘＡの逆数の平方根で設定する（Ｓ１６）。それから、ステップＳ１０に移行する。

以上のように第２実施形態によれば、指令値補正部２２は、アームの各軸についてバネ定数Ｋ１〜Ｋ６を演算すると共に、アームの各軸についてイナーシャＩ１〜Ｉ６を演算する。そして、補正係数Ｉ／Ｋが最大となるものとＭａｘＡとして選択し、その最大補正係数に基づく補正量Ａｄｊにより各軸の指令値θｎを修正するようにした。このような制御による場合も、第１実施形態と同様に手先の振動を抑制できる。また、イナーシャＩをバネ定数Ｋで除した補正係数に応じてローパスフィルタの遮断周波数を可変設定するので、それらの値を考慮して遮断周波数を適切に設定できる。

（第３実施形態）
第３実施形態では、バネ定数Ｋを（７）式により求めることに替えて、１次近似式を用いて求める。図１３は、バネ定数Ｋを（７）式で求めた場合の一例であり、横軸に距離Ｌ_ｆの２乗を取ると、曲線を描く。図１３は、距離Ｌ_ｆの値域は３００ｍｍ〜１５００ｍｍであるが、図１４に示すように、手先の振動が顕著になる９００ｍｍ以上の領域とそれ未満の領域とを分けてそれぞれの領域に１次近似を適用すると、少ない誤差で近似できる。

そして、１次近似で求めた２つのバネ定数Ｋ１，Ｋ２による連立方程式を解くことで、２つの直線の交点が求められる。その交点で、２つのバネ定数Ｋ１，Ｋ２の適用を切り替えるようにすれば良い。図１４に示す例では、交点はＬ_ｆ＝７６７ｍｍとなっている。
以上のように第３実施形態によれば、指令値補正部２２は、バネ定数Ｋを１次近似式により演算するので、指令値の補正に使用するバネ定数Ｋをより簡単に得ることができる。

本発明は上記した、又は図面に記載した実施形態にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
ステップＳ９，Ｓ１６におけるローパスフィルタ処理は、必要に応じて行えば良い。
第１実施形態におけるバネ定数Ｋは、（７）式以外の式によって求めても良い。
第３実施形態における一次近似は、３以上の領域に分けて近似を行っても良い。

図面中、１はロボットシステム、２はロボット、３はコントローラ、１３はワーク、２１は軌道生成部、２２は指令値補正部を示す。

Claims (7)

1. 設置面に対し垂直となる第１軸と、この第１軸に対して垂直となる第２軸と、この第２軸以降に連結される複数の軸のうち、少なくとも１つが前記第２軸に対して平行となる軸を備える４軸以上のアームと、
   このアームをサーボ機構を介して駆動制御するロボットの制御装置であって、
   前記アームの手先を移動させる最終目標位置が与えられると、前記手先を前記最終目標位置に移動させる速度パターンを生成する速度パターン生成部と、
   前記手先を、制御周期毎の通過目標位置に移動させるように、前記サーボ機構に与える指令値を算出する指令値算出部と、
   前記手先の現在位置に応じて、前記アームのバネ定数を演算するバネ定数演算部と、
   前記手先の現在位置に応じて、前記アームと前記手先に保持されるワークとについてのイナーシャを演算するイナーシャ演算部と、
   前記指令値が前記制御周期毎に変化する際の加速度を算出する加速度算出部と、
   前記バネ定数、前記イナーシャ及び前記加速度から、前記指令値の補正量を演算する補正量演算部と、
   前記指令値を、前記補正量に基づき補正して前記サーボ機構に出力する指令値補正部とを備えるロボットの制御装置。
2. 前記バネ定数演算部は、前記アーム全体のバネ定数を演算し、
   前記イナーシャ演算部は、前記アーム全体及び前記ワークを含むイナーシャを演算する請求項１記載のロボットの制御装置。
3. 前記バネ定数演算部は、第３軸から前記手先までの距離をＬ_３，
   前記第１軸から前記手先までの距離をＬ_ｆ，
   前記第１軸を基準とする前記第３軸までのバネ定数をＫ_１２，
   前記第３軸を基準とする前記手先までのバネ定数をＫ_３とすると、
   前記補正量を求めるために使用するバネ定数Ｋを、次式
   １／Ｋ＝｛１／Ｋ_１２＋Ｌ_３ ^２／（Ｋ_３Ｌ_ｆ ^２）｝
   で求める請求項２記載のロボットの制御装置。
4. 前記バネ定数演算部は、前記アームの各軸を基準とするバネ定数を演算し、
   前記イナーシャ演算部は、前記アームの各軸を基準とするイナーシャを演算し、
   前記補正量演算部は、各軸に対応するイナーシャを各軸に対応するバネ定数で除すことで、各軸に対応する補正係数を求め、前記補正係数のうち最大値を示したものを用いて各軸に適用する補正量を決定する請求項１記載のロボットの制御装置。
5. 前記補正量演算部は、前記バネ定数を１次近似式により演算する請求項１から４の何れか一項に記載のロボットの制御装置。
6. 前記指令値補正部は、補正した指令値に対しローパスフィルタ処理を行う請求項１から５の何れか一項に記載のロボットの制御装置。
7. 前記指令値補正部は、前記イナーシャを前記バネ定数で除した補正係数に応じて、前記ローパスフィルタ処理の遮断周波数を可変設定する請求項６記載のロボットの制御装置。

Images