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

Description

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

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

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

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

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

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

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

そこで、バネ定数演算部は、位置指令値に応じてアームのバネ定数を演算する。加速度算出部は、位置指令値が制御周期毎に変化する際の加速度を算出する。補正量演算部は、バネ定数，アーム及び手先に保持されるワークの質量並びに加速度から指令値の補正量を演算すると、指令値補正部は、前記補正量に基づきした位置指令値を角度指令値に逆変換してサーボ機構に出力する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

次に、指令値補正部２２は、各軸の目標角度から手先の目標位置Ｐを求めると（Ｓ４）、その目標位置Ｐに応じたバネ定数Ｋを（４）式で求める。また、アームを含むワーク１３の質量Ｍを予め記憶しておいたメモリから取得する（Ｓ５）。尚、目標位置Ｐを求める際に、（４）式の演算パラメータである距離Ｌ_３及びＬ_ｆも求められる。目標位置Ｐは通過目標位置に相当する。続いて、各軸の速度パターンから、各軸がそれぞれ目標角度に至る間の角加速度ａを計算する（Ｓ６）。

次に、指令値補正部２２は、位置の補正量（Ｍａ／Ｋ）を計算する（Ｓ７）。そして、手先の位置指令値Ｐに補正量（Ｍａ／Ｋ）を加えた修正指令値Ｐ’を計算すると（Ｓ８）、その修正指令値Ｐ’に対してローパスフィルタ処理を行う（Ｓ９）。修正指令値Ｐ’から角度に対応する指令値θ’を各軸について計算すると（１０）、指令値θ’をサーボ機構２３に転送する（Ｓ１１）。それから、ステップＳ３に戻る。指令値補正部２２は、指令値変換部，バネ定数演算部，加速度算出部及び補正量演算部に相当する。

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

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

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

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

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

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

以上のように本実施形態によれば、軌道生成部２１は、アームの手先を移動させる最終目標位置が与えられると、その手先を最終目標位置に移動させる速度パターンを生成し、手先をサンプリング周期毎の目標位置に移動させるように、サーボ機構２３に与える角度指令値θｎを算出する。指令値補正部２２は、角度指令値θｎを手先の目標位置，位置指令値Ｐに変換し、その位置指令値Ｐに応じてアームのバネ定数Ｋを演算し、位置指令値Ｐがサンプリング周期毎に変化する際の加速度ａを算出する。そして、これらの値から補正量（Ｍａ／Ｋ）を演算すると、位置指令値Ｐに補正量を加算して修正指令値Ｐ’を求め、修正指令値Ｐ’を位置指令値指令値θｎ’に逆変換してサーボ機構２３に出力する。

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

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

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

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

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

Claims (5)

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

Images