WO2020255724A1

WO2020255724A1 - ロボット制御装置及びロボット制御方法

Info

Publication number: WO2020255724A1
Application number: PCT/JP2020/022026
Authority: WO
Inventors: 鉄也田原; 陽一大川
Original assignee: アズビル株式会社
Priority date: 2019-06-17
Filing date: 2020-06-03
Publication date: 2020-12-24
Also published as: JP2020203340A; JP7273627B2; CN114007820A

Abstract

力又はトルクの指令値を低周波成分及び高周波成分に分離する周波数分離部（１１１）と、力又はトルクの現在値を低周波成分及び高周波成分に分離する周波数分離部（１２２）と、周波数分離部（１１１）による高周波成分に基づいてトルクの指令値の高周波成分を演算し、周波数分離部（１１１，１２２）による低周波成分に基づいて力制御の指令値を演算し、当該力制御の指令値に基づいて制御指令値を演算するメイン制御部（１１）と、関節毎に設けられ、周波数分離部（１２２）による高周波成分及びメイン制御部（１１）によるトルクの指令値の高周波成分に基づいてトルク制御の指令値を演算し、当該トルク制御の指令値及び当該メイン制御部（１１）による制御指令値に基づいてモータ（２１）に対する指令値を演算する関節制御部（１２）とを備え、周波数分離部（１１１，１２２）はメイン制御部（１１）及び関節制御部（１２）の外部又は内部に設けられた。

Description

ロボット制御装置及びロボット制御方法

　この発明は、ロボットの位置姿勢と力を同時に制御可能なロボット制御装置及びロボット制御方法に関する。

　垂直多関節ロボット等のロボット（ロボットアーム）では、位置姿勢と力を同時（並列）に制御対象とするロボット制御装置が用いられている（例えば特許文献１参照）。なお、位置姿勢は、ロボットの位置及び姿勢のうちの少なくとも一方を表す。図１３～図１５に、ロボット制御装置１ｂの一例を示す。

　図１３に示すロボット制御装置１ｂは、メイン制御部（上位コントローラ）１１ｂ、及び複数の関節制御部（下位コントローラ）１２ｂを備えている。関節制御部１２ｂは、ロボット２が有する関節毎に設けられている。なお、メイン制御部１１ｂと各関節制御部１２ｂとの間は通信線により接続されている。

　また、図１３に示すように、ロボット２は、関節毎に、モータ２１及びセンサ２２（トルクセンサ２３及びエンコーダ２４）を有している。モータ２１及びセンサ２２はそれぞれ、対応する関節制御部１２ｂに対して電力線等により接続されている。トルクセンサ２３は、対応する関節のトルクの現在値を検出する。エンコーダ２４は、対応する関節の角度の現在値を検出する。なお図１４では、モータ２１、トルクセンサ２３及びエンコーダ２４を一組のみ示している。

　メイン制御部１１ｂは、各関節制御部１２ｂに指令値を出力することで、ロボット２全体を制御する。具体的には、メイン制御部１１ｂは、力の指令値及び位置姿勢の指令値並びにロボット２が有する関節毎のトルクの現在値及び角度の現在値に基づいて、当該関節毎の角速度の指令値を演算する。メイン制御部１１ｂは、図１４及び図１５に示すように、力演算部１１１ｂ、力制御部１１２ｂ、位置姿勢演算部１１３ｂ、位置姿勢制御部１１４ｂ、指令値合成部１１５ｂ及び指令値変換部１１６ｂを備えている。

　力演算部１１１ｂは、ロボット２が有する関節毎のトルクの現在値に基づいて、ロボット２の力の現在値を演算する。ロボット２が有する関節毎のトルクは関節座標系で表されており、この力演算部１１１ｂでは、係数乗算部１１１１ｂで関節毎のトルクを並べたベクトルにヤコビ行列の転置の逆行列を乗算して、関節毎のトルクを直交座標系で表された力に変換する。図１５において、τはトルクの現在値を表し、Ｊはヤコビ行列を表し、Ｆは力の現在値を表している。

　力制御部１１２ｂは、力の指令値及び力演算部１１１ｂにより演算された力の現在値に基づいて、力制御の指令値を演算する。この力制御部１１２ｂでは、偏差演算器１１２１ｂで力の指令値と力の現在値の間の偏差を求め、係数乗算部１１２２ｂで偏差演算器１１２１ｂによる演算結果の偏差に対してゲインを乗算することで、力制御の指令値を得る。図１５において、Ｆｒは力の指令値を表し、Ｇ_Ｆはゲインを表している。

　位置姿勢演算部１１３ｂは、ロボット２が有する関節毎の角度の現在値に基づいて、ロボット２の位置姿勢の現在値を演算する。ロボット２が有する関節毎の角度の現在値は関節座標系で表されており、位置姿勢演算部１１３ｂは、関節毎の角度の現在値を直交座標系で表された位置姿勢の現在値に変換する。図１５において、θは角度の現在値を表し、Ｘは位置姿勢の現在値を表している。

　位置姿勢制御部１１４ｂは、位置姿勢の指令値及び位置姿勢演算部１１３ｂにより演算された位置姿勢の現在値に基づいて、位置姿勢制御の指令値を演算する。この位置姿勢制御部１１４ｂでは、偏差演算器１１４１ｂで位置姿勢の指令値と位置姿勢の現在値の間の偏差を求め、係数乗算部１１４２ｂで偏差演算器１１４１ｂによる演算結果に対してゲインを乗算することで、位置姿勢制御の指令値を得る。図１５において、Ｘｒは位置姿勢の指令値を表し、Ｇ_Ｚはゲインを表している。

　指令値合成部１１５ｂは、力制御部１１２ｂにより演算された力制御の指令値及び位置姿勢制御部１１４ｂにより演算された位置姿勢制御の指令値を合成する。この指令値合成部１１５ｂでは、加算器１１５１ｂで力制御の指令値と位置姿勢制御の指令値とを加算する。

　指令値変換部１１６ｂは、指令値合成部１１５ｂによる合成結果を、ロボット２が有する関節毎の角速度の指令値に変換する。この指令値変換部１１６ｂでは、係数乗算部１１６１ｂで上記合成結果に対してヤコビ行列の逆行列を乗算する。すなわち、指令値変換部１１６ｂは、直交座標系で表された指令値を関節座標系で表された指令値に変換する。図１５において、θ（ドット）ｒは角速度の指令値を表している。

　関節制御部１２ｂは、メイン制御部１１ｂからの指令に応じ、対応する関節に設けられたモータ２１を制御する。関節制御部１２ｂは、図１４に示すように、トルク取得部１２１ｂ及び関節角制御部１２２ｂを備えている。

　トルク取得部１２１ｂは、対応する関節でのトルクの現在値を取得する。このトルク取得部１２１ｂにより取得されたトルクの現在値を示すデータは、メイン制御部１１ｂ（力演算部１１１ｂ）に出力される。

　関節角制御部１２２ｂは、メイン制御部１１ｂにより演算された角速度の指令値及びロボット２が有する関節毎の角度の現在値に基づいて、対応する関節に設けられたモータ２１に対する指令値を演算する。この関節角制御部１２２ｂでは、速度変換部１２２１ｂで角度の現在値を角速度の現在値に変換し、減算器１２２３ｂで角速度の指令値から速度変換部１２２１ｂで得られた角速度の現在値を減算し、ＰＩ制御部１２２４ｂで減算器１２２３ｂによる減算結果に基づいてＰＩ制御を行うことで、モータ２１に対する指令値を得る。

　このように、図１３～図１５に示すロボット制御装置１ｂでは、複数の関節を連携して操作する必要があり、メイン制御部１１ｂで、多関節の自由度（例えば６自由度）に対して位置姿勢と力を同時に制御演算した結果を合成し、当該合成結果を各軸の関節制御部１２ｂへの信号に変換してから出力する。すなわち、このロボット制御装置１ｂでは、メイン制御部１１ｂでコンプライアンス制御の主要な演算を実行する。そのため、このロボット制御装置１ｂでは、調整すべきパラメータを合理的に統合できる等のメリットがある。

特開２０１６－１６８６５０号公報

　一般に、産業用ロボット等のロボットでは、力制御によって実現が求められる対象として精密な研磨の倣い動作等があり、整定動作又は追従動作のようなダイナミクスの性能向上が常に求められている。
　一方、従来のロボット制御装置では、メイン制御部でフィードバック系を構成している。すなわち、このロボット制御装置では、ロボットから物理的にも通信的にも距離のある構成要素でフィードバック制御演算を行うことになる。よって、トルクセンサによるトルクの検出からモータへの指令値の入力までの遅延が長くなる。その結果、このロボット制御装置では、不可避的にむだ時間が入り込む余地が多くなり、安定性を維持できるハイゲイン化を抑制する要因になる。また、むだ時間自体も、リード補償等でキャンセルできる要素ではないため、応答時間への悪影響は避けられない。
　このように、従来のロボット制御装置では、力制御の性能（特に速応性）の向上が難しく、更なる改善が求められている。

　この発明は、上記のような課題を解決するためになされたもので、従来構成に対して力制御の性能を向上可能であるロボット制御装置を提供することを目的としている。

　この発明に係るロボット制御装置は、ロボットの力の指令値又は当該ロボットが有する関節毎のトルクの指令値を低周波成分及び高周波成分に分離する第１周波数分離部と、ロボットの力の現在値又は当該ロボットが有する関節毎のトルクの現在値を低周波成分及び高周波成分に分離する第２周波数分離部と、第１周波数分離部により得られた高周波成分に基づいてロボットが有する関節毎のトルクの指令値の高周波成分を演算し、第１周波数分離部により得られた低周波成分及び第２周波数分離部により得られた低周波成分に基づいて力制御の指令値を演算し、当該力制御の指令値に基づいてロボットが有する関節毎の制御指令値を演算するメイン制御部と、ロボットが有する関節毎に設けられ、第２周波数分離部により得られた高周波成分及びメイン制御部により演算されたトルクの指令値の高周波成分に基づいてトルク制御の指令値を演算し、当該トルク制御の指令値及び当該メイン制御部により演算された制御指令値に基づいて対応する関節に設けられたモータに対する指令値を演算する関節制御部とを備え、第１周波数分離部及び第２周波数分離部は、メイン制御部及び関節制御部の外部又は内部に設けられたことを特徴とする。

　この発明によれば、上記のように構成したので、従来構成に対して力制御の性能を向上可能となる。

実施の形態１に係るロボット制御装置の構成例を示す図である。実施の形態１に係るロボット制御装置の構成例を示す図である。図３Ａ、図３Ｂは、実施の形態１における周波数分離部の構成例を示す図である。実施の形態１に係るロボット制御装置の動作例を示すフローチャートである。実施の形態１におけるメイン制御部の動作例を示すフローチャートである。実施の形態１における関節制御部の動作例を示すフローチャートである。実施の形態１に係るロボット制御装置による低周波力制御と高周波トルク制御の一例を示す図である。実施の形態１に係るロボット制御装置の別の構成例を示す図である。実施の形態２に係るロボット制御装置の構成例を示す図である。実施の形態２に係るロボット制御装置の構成例を示す図である。実施の形態３に係るロボット制御装置の構成例を示す図である。実施の形態３における周波数分離部の構成例を示す図である。従来のロボット制御装置を含むロボットシステムの構成例を示す図である。従来のロボット制御装置の構成例を示す図である。従来のロボット制御装置の構成例を示す図である。

　以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
　図１，２は実施の形態１に係るロボット制御装置１の構成例を示す図である。なお、ロボット制御装置１とロボット２との関係は、図１３と同様であり、その説明を省略する。
　ロボット制御装置１は、ロボット２の位置姿勢と力を同時（並列）に制御する。ロボット制御装置１は、図１，２に示すように、メイン制御部（上位コントローラ）１１、及び複数の関節制御部（下位コントローラ）１２を備えている。関節制御部１２は、ロボット２が有する関節毎に設けられている。なお、メイン制御部１１と各関節制御部１２との間は通信線により接続されている。

　メイン制御部１１は、各関節制御部１２に指令値を出力することで、ロボット２全体を制御する。具体的には、メイン制御部１１は、ロボット２の力の指令値及び位置姿勢の指令値並びにロボット２が有する関節毎の角度の現在値及びトルクの現在値の低周波成分に基づいて、当該関節毎のトルクの指令値の高周波成分及び制御指令値を演算する。図１，２では、メイン制御部１１が演算する制御指令値は角速度の指令値である。メイン制御部１１は、図１に示すように、周波数分離部（第１周波数分離部）１１１、トルク指令値変換部１１２、力演算部１１３、力制御部１１４、位置姿勢演算部１１５、位置姿勢制御部１１６、指令値合成部１１７、及び指令値変換部１１８を備えている。ただし、位置姿勢演算部１１５、位置姿勢制御部１１６、指令値合成部１１７は、力と位置姿勢の双方を制御する場合に必要であり、力だけを制御する場合は不要である。なお、メイン制御部１１は、システムＬＳＩ（Ｌａｒｇｅ　Ｓｃａｌｅ　Ｉｎｔｅｇｒａｔｉｏｎ）等の処理回路、又はメモリ等に記憶されたプログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）等により実現される。

　周波数分離部１１１は、力の指令値を低周波成分及び高周波成分に分離する。図１，２において、Ｆｒは力の指令値を表している。
　なお、周波数分離部１１１は、低周波域と高周波域を、例えば、トルクセンサ２３によるトルクの検出からモータ２１への指令値の入力までの遅延量に基づいて、設定する。例えば、周波数分離部１１１は、上記遅延量が５ｍｓ程度であった場合、その１０倍である５０ｍｓ程度を境にして低周波域と高周波域を設定する。後述する周波数分離部１２２についても同様である。

　トルク指令値変換部１１２は、周波数分離部１１１により得られた力の指令値の高周波成分を、ロボット２が有する関節毎のトルクの指令値の高周波成分に変換する。トルク指令値変換部１１２は、係数乗算部１１２１を有している。係数乗算部１１２１は、力の指令値の高周波成分に対してヤコビ行列の転置行列を乗算する。力の指令値の高周波成分は直交座標系で表されており、トルク指令値変換部１１２は、力の指令値の高周波成分を関節座標系で表されたトルクの指令値の高周波成分に変換する。図２において、Ｊはヤコビ行列を表し、τｒはトルクの指令値を表している。

　力演算部１１３は、ロボット２が有する関節毎のトルクの現在値の低周波成分を、力の現在値の低周波成分に変換する。力演算部１１３は、係数乗算部１１３１を有している。係数乗算部１１３１は、トルクの現在値の低周波成分に対してヤコビ行列の転置の逆行列を乗算する。トルクの現在値の低周波成分は関節座標系で表された値であるが、力制御部１１４では直交座標系で表された力の指令値が必要であるため、力演算部１１３によって変換する。なお、ロボット２が有する関節毎のトルクの現在値の低周波成分は、周波数分離部１２２により得られる。

　力制御部１１４は、周波数分離部１１１により得られた力の指令値の低周波成分及び力演算部１１３により得られた力の現在値の低周波成分に基づいて、速度の指令値（低周波力制御の指令値）を演算する。この力制御部１１４は、力制御のうちの低周波域の制御（定常応答の制御）を実行することになる。力制御部１１４は、減算器１１４１及び係数乗算部１１４２を有している。

　減算器１１４１は、力の指令値の低周波成分と力の現在値の低周波成分との間の偏差を減算によって求める。
　係数乗算部１１４２は、減算器１１４１により求められた偏差に対してゲインを乗算することで、速度の指令値を得る。図２において、Ｇ_Ｆは力の偏差に対するゲインを表している。

　位置姿勢演算部１１５は、ロボット２が有する関節毎の角度の現在値に基づいて、ロボット２の位置姿勢の現在値を演算する。ロボット２が有する関節毎の角度は関節座標系で表されており、位置姿勢演算部１１５は、関節毎の角度を直交座標系で表された位置姿勢に変換する。なお、ロボット２が有する関節毎の角度の現在値は、当該関節毎に設けられたエンコーダ２４により検出される。図２において、θは角度の現在値を表し、Ｘは位置姿勢の現在値を表している。

　位置姿勢制御部１１６は、位置姿勢の指令値及び位置姿勢演算部１１５により演算された位置姿勢の現在値に基づいて、速度の指令値（位置姿勢制御の指令値）を演算する。位置姿勢制御部１１６は、偏差演算器１１６１及び係数乗算部１１６２を有している。

　偏差演算器１１６１は、位置姿勢の指令値と位置姿勢の現在値との間の偏差を演算によって求める。
　係数乗算部１１６２は、偏差演算器１１６１による演算結果の偏差に対してゲインを乗算することで、速度の指令値を得る。図２において、Ｘｒは位置姿勢の指令値を表し、Ｇ_Ｚはゲインを表している。

　指令値合成部１１７は、力制御部１１４により演算された速度の指令値と位置姿勢制御部１１６により演算された速度の指令値とを加算することで合成し、１つの速度の指令値を得る。指令値合成部１１７は、加算器１１７１を有する。加算器１１７１は、力制御部１１４により演算された速度の指令値と位置姿勢制御部１１６により演算された速度の指令値とを加算する。

　指令値変換部１１８は、指令値合成部１１７により得られた速度の指令値を、ロボット２が有する関節毎の角速度の指令値に変換する。指令値変換部１１８は、係数乗算部１１８１を有している。係数乗算部１１８１は、指令値合成部１１７により得られた速度の指令値に対してヤコビ行列の逆行列を乗算する。すなわち、指令値変換部１１８は、直交座標系で表された指令値を関節座標系で表された指令値に変換する。図２において、θ（ドット）ｒは角速度の指令値を表している。

　関節制御部１２は、メイン制御部１１からの指令に応じ、対応する関節に設けられたモータ２１を制御する。具体的には、関節制御部１２は、対応する関節でのトルクの現在値並びにメイン制御部１１により演算されたトルクの指令値の高周波成分及び制御指令値に基づいて、対応する関節に設けられたモータ２１に対する指令値を演算する。図１，２では、上記制御指令値は角速度の指令値である。関節制御部１２は、図１に示すように、トルク取得部１２１、周波数分離部（第２周波数分離部）１２２、トルク制御部１２３及びモータ制御部１２４を備えている。モータ制御部１２４は、関節角制御部１２５及び指令値合成部１２６を有している。

　トルク取得部１２１は、対応する関節でのトルクの現在値を取得する。ロボット２が有する関節毎のトルクの現在値は、当該関節毎に設けられたトルクセンサ２３により検出される。

　周波数分離部１２２は、トルク取得部１２１により取得されたトルクの現在値を低周波成分及び高周波成分に分離する。

　トルク制御部１２３は、周波数分離部１２２により得られたトルクの現在値の高周波成分及びメイン制御部１１により演算されたトルクの指令値の高周波成分に基づいて、トルク制御の指令値を演算する。このトルク制御部１２３は、力制御のうちの高周波域の制御（過渡応答の制御）を実行することになる。トルク制御部１２３は、減算器１２３１及びＰＩ制御部１２３２を有している。

　減算器１２３１は、メイン制御部１１により演算されたトルクの指令値の高周波成分から、周波数分離部１２２により得られたトルクの現在値の高周波成分を減算する。
　ＰＩ制御部１２３２は、減算器１２３１による減算結果に基づいてＰＩ制御を行うことで、トルク制御の指令値を得る。

　関節角制御部１２５は、メイン制御部１１により演算された角速度の指令値に基づいて、角速度制御の指令値を演算する。関節角制御部１２５は、速度変換部１２５１及び速度制御部１２５２を有している。速度制御部１２５２は、減算器１２５３及びＰＩ制御部１２５４を有している。

　速度変換部１２５１は、対応する関節での角度の現在値を角速度の現在値に変換する。

　減算器１２５３は、メイン制御部１１により演算された角速度の指令値から、速度変換部１２５１により得られた角速度の現在値を減算する。
　ＰＩ制御部１２５４は、減算器１２５３による減算結果に基づいてＰＩ制御を行うことで、角速度制御の指令値を得る。

　指令値合成部１２６は、トルク制御部１２３により演算されたトルク制御の指令値及び関節角制御部１２５により演算された角速度制御の指令値を合成する。図２では、指令値合成部１２６は、加算器１２６１を有する。加算器１２６１は、トルク制御部１２３により演算されたトルク制御の指令値と関節角制御部１２５により演算された角速度制御の指令値とを加算する。この指令値合成部１２６による合成結果である指令値（電流指令値）は、モータ２１に出力される。

　次に、周波数分離部１１１の構成例について、図３を参照しながら説明する。図３では周波数分離部１１１の構成例を示しているが、周波数分離部１２２についても同様である。
　図３Ａに示す周波数分離部１１１は、ローパスフィルタ１１１１及びハイパスフィルタ１１１２を有している。
　ローパスフィルタ１１１１は、外部から入力された信号の低周波成分のみを通す。
　ハイパスフィルタ１１１２は、外部から入力された信号の高周波成分のみを通す。
　なお、ローパスフィルタ１１１１とハイパスフィルタ１１１２は、カットオフ周波数が同一（略同一の意味を含む）であることが望ましい。

　図３Ｂに示す周波数分離部１１１は、ローパスフィルタ１１１３及び減算器１１１４を有している。
　ローパスフィルタ１１１３は、外部から入力された信号の低周波成分のみを通す。
　減算器１１１４は、外部から入力された信号からローパスフィルタ１１１３を通過した信号を減算する。減算器１１１４により得られる信号は、外部から入力された信号の高周波成分である。

　なお図３では、周波数分離部１１１がローパスフィルタ１１１３を有する場合を示した。しかしながら、これに限らず、周波数分離部１１１は、ローパスフィルタ１１１３の代わりに、重み付き移動平均等のように平滑化効果のある他の信号処理手法を用いた構成を有していてもよい。

　次に、図１，２に示す実施の形態１に係るロボット制御装置１の動作例について、図４を参照しながら説明する。
　図１，２に示す実施の形態１に係るロボット制御装置１の動作例では、図４に示すように、まず、メイン制御部１１は、ロボット２の力の指令値及び位置姿勢の指令値並びにロボット２が有する関節毎の角度の現在値及びトルクの現在値の低周波成分に基づいて、当該関節毎のトルクの指令値の高周波成分及び角速度の指令値を演算する（ステップＳＴ４０１）。

　次いで、関節制御部１２は、対応する関節でのトルクの現在値並びにメイン制御部１１により演算されたトルクの指令値の高周波成分及び角速度の指令値に基づいて、対応する関節に設けられたモータ２１に対する指令値を演算する（ステップＳＴ４０２）。

　次に、図１，２に示すメイン制御部１１の動作例について、図５を参照しながら説明する。
　図１，２に示すメイン制御部１１の動作例では、図５に示すように、まず、周波数分離部１１１は、力の指令値を低周波成分及び高周波成分に分離する（ステップＳＴ５０１）。

　次いで、トルク指令値変換部１１２は、周波数分離部１１１により得られた力の指令値の高周波成分を、ロボット２が有する関節毎のトルクの指令値の高周波成分に変換する（ステップＳＴ５０２）。図１，２では、係数乗算部１１２１が、力の指令値の高周波成分に対してヤコビ行列の転置行列を乗算する。なお、ヤコビ行列はロボット２の関節の角度によって変わるので、適宜更新する必要がある。

　また、力演算部１１３は、ロボット２が有する関節毎のトルクの現在値の低周波成分を、力の現在値の低周波成分に変換する（ステップＳＴ５０３）。図１，２では、係数乗算部１１３１が、トルクの現在値の低周波成分に対してヤコビ行列の転置の逆行列を乗算する。なお、トルク取得部１２１が取得するトルクの現在値は通常、重力に起因するトルク成分を含むので、トルクから力へ変換する前に、トルクの現在値の低周波成分からこの重力起因トルク成分の推定値を減算して除去すると良い。

　次いで、力制御部１１４は、周波数分離部１１１により得られた力の指令値の低周波成分及び力演算部１１３により得られた力の現在値の低周波成分に基づいて、速度の指令値（低周波力制御の指令値）を演算する（ステップＳＴ５０４）。図１，２では、減算器１１４１が、力の指令値の低周波成分と力の現在値の低周波成分との間の偏差を減算によって求め、係数乗算部１１４２が、減算器１１４１により求められた偏差に対してゲインを乗算することで、速度の指令値を得る。

　また、位置姿勢演算部１１５は、ロボット２が有する関節毎の角度の現在値に基づいて、ロボット２の位置姿勢の現在値を演算する（ステップＳＴ５０５）。

　次いで、位置姿勢制御部１１６は、位置姿勢の指令値及び位置姿勢演算部１１５により演算された位置姿勢の現在値に基づいて、速度の指令値（位置姿勢制御の指令値）を演算する（ステップＳＴ５０６）。図１，２では、偏差演算器１１６１が、位置姿勢の指令値と位置姿勢の現在値との間の偏差を演算し、係数乗算部１１６２が、偏差演算器１１６１による演算結果の偏差に対してゲインを乗算することで、速度の指令値を得る。なお、位置の偏差は、指令値の座標値から現在値の座標値を減算することで得られる。姿勢の偏差は、現在値の姿勢から指令値の姿勢への回転変換を求めることで得ることができる。

　次いで、指令値合成部１１７は、力制御部１１４により演算された速度の指令値と位置姿勢制御部１１６により演算された速度の指令値とを加算することで合成し、１つの速度の指令値を得る（ステップＳＴ５０７）。

　次いで、指令値変換部１１８は、指令値合成部１１７により得られた速度の指令値を、ロボット２が有する関節毎の角速度の指令値に変換する（ステップＳＴ５０８）。図１，２では、係数乗算部１１８１が、指令値合成部１１７により得られた速度の指令値に対してヤコビ行列の逆行列を乗算することで、関節毎の角速度の指令値を得る。

　次に、図１，２に示す関節制御部１２の動作例について、図６を参照しながら説明する。
　図１，２に示す関節制御部１２の動作例では、図６に示すように、まず、トルク取得部１２１は、対応する関節でのトルクの現在値を取得する（ステップＳＴ６０１）。

　次いで、周波数分離部１２２は、トルク取得部１２１により取得されたトルクの現在値を低周波成分及び高周波成分に分離する（ステップＳＴ６０２）。

　次いで、トルク制御部１２３は、周波数分離部１２２により得られたトルクの現在値の高周波成分及びメイン制御部１１により演算されたトルクの指令値の高周波成分に基づいて、トルク制御の指令値を演算する（ステップＳＴ６０３）。図１，２では、減算器１２３１が、メイン制御部１１により演算されたトルクの指令値の高周波成分から周波数分離部１２２により得られたトルクの現在値の高周波成分を減算し、ＰＩ制御部１２３２が、減算器１２３１による減算結果に基づいてＰＩ制御を行うことで、トルク制御の指令値を得る。

　また、関節角制御部１２５は、メイン制御部１１により演算された角速度の指令値に基づいて、角速度制御の指令値を演算する（ステップＳＴ６０４）。図１，２では、速度変換部１２５１が、対応する関節での角度の現在値を角速度の現在値に変換し、減算器１２５３が、メイン制御部１１により演算された角速度の指令値から速度変換部１２５１により得られた角速度の現在値を減算し、ＰＩ制御部１２５４が、減算器１２５３による減算結果に基づいてＰＩ制御を行うことで、角速度制御の指令値を得る。

　次いで、指令値合成部１２６は、トルク制御部１２３により演算されたトルク制御の指令値及び関節角制御部１２５により演算された角速度制御の指令値を合成する（ステップＳＴ６０５）。図１，２では、加算器１２６１が、トルク制御部１２３により演算されたトルク制御の指令値と関節角制御部１２５により演算された角速度制御の指令値とを加算する。この指令値合成部１２６による合成結果である指令値（電流指令値）は、モータ２１に出力される。

　次に、実施の形態１に係るロボット制御装置１による力制御について説明する。
　図７は、実施の形態１に係るロボット制御装置１による低周波力制御と高周波トルク制御の一例を示す図である。図７において、太線実線矢印は力又はトルクの低周波成分の流れを示し、細線実線矢印は力又はトルクの高周波成分の流れを示している。また、太線実線矢印と細線実線矢印とが並列している箇所は、力又はトルクが周波数で分離されていない箇所を示している。また、破線矢印は位置姿勢制御のみに関連する箇所を示している。

　図７において、低周波成分に関しては、力制御部１１４で、力の現在値の低周波成分が力の指令値の低周波成分に一致するように制御される。力の現在値の低周波成分は、トルクの現在値が周波数分離部１２２で周波数分離されたものが力演算部１１３で力に変換されることで得られる。また、力の指令値の低周波成分は、力の指令値が周波数分離部１１１で周波数分離されることで得られる。そして、力制御部１１４で生成した指令値は、指令値合成部１１７、指令値変換部１１８、速度制御部１２５２及び指令値合成部１２６を経て、モータ２１を駆動する。このようにして、実施の形態１に係るロボット制御装置１では、力の低周波成分が制御され、定常応答が制御される。

　また、図７において、高周波成分に関しては、トルク制御部１２３で、トルクの現在値の高周波成分がトルクの指令値の高周波成分に一致するように制御される。トルクの現在値の高周波成分は、トルクの現在値が周波数分離部１２２で周波数分離されることで得られる。トルクの指令値の高周波成分は、力の指令値が周波数分離部１１１で周波数分離されたものがトルク指令値変換部１１２で変換されることで得られる。トルク制御部１２３が生成した指令値は、指令値合成部１２６を経て、モータ２１を駆動する。このようにして、実施の形態１に係るロボット制御装置１では、力の高周波成分が制御され、過渡応答が制御される。

　そして、実施の形態１に係るロボット制御装置１では、上記の２つの制御が指令値合成部１２６で合成されることで、全体として力の指令値に力の現在値が一致するように制御される。

　次に、実施の形態１に係るロボット制御装置１による効果について説明する。
　上述したように、従来のロボット制御装置１ｂでは、メイン制御部１１ｂでフィードバック系を構成している。すなわち、このロボット制御装置１ｂでは、ロボット２から物理的にも通信的にも距離のある構成要素でフィードバック制御演算を行うことになる。よって、トルクセンサ２３によるトルクの検出からモータ２１への指令値の入力までの遅延が長くなる。その結果、このロボット制御装置１ｂでは、不可避的にむだ時間が入り込む余地が多くなり、安定性を維持できるハイゲイン化を抑制する要因になる。また、むだ時間自体も、リード補償等でキャンセルできる要素ではないため、応答時間への悪影響は避けられない。

　これに対し、実施の形態１に係るロボット制御装置１では、力制御の実行を過渡応答の制御（高周波域）と定常応答の制御（低周波域）に分割し、過渡応答の制御については、関節制御部１２に近い側に配設可能なトルクセンサ２３の値を制御することで実現し、下位コントローラ（図中の関節制御部１２）側で制御の主要な演算を実行する。これにより、実施の形態１に係るロボット制御装置１では、むだ時間が入り込む余地を削減して速応性を高められる。すなわち、実施の形態１に係るロボット制御装置１では、安定性を維持できるコントローラのハイゲイン化に相当する調整（関節単位の１変数制御のゲイン調整）も可能になる。
　一方で、実施の形態１に係るロボット制御装置１では、定常応答の制御は従来同様に上位コントローラ（メイン制御部１１）で、複数の関節を連携して制御する。これにより、実施の形態１に係るロボット制御装置１では、定常偏差のような定常的な制御特性は従来と同様になる。すなわち、関節制御部１２は関節単位の制御であり、定常的な制御偏差が発生する場合がある。例えば、Ｚ軸方向に力制御を行っている際にＸ軸方向の外力が外乱として加わると、Ｚ軸方向の力の現在値と目標値との間に偏差が生じるといったことが起こる。このような外乱は関節単位の制御では抑制できないため、制御偏差が生じる。これに対し、実施の形態１に係るロボット制御装置１では、定常応答の制御をメイン制御部１１で行っており、上記課題を解決できる。そして、速応性に影響するのは主に過渡応答の制御（高周波域）なので、従来技術が持っていた速応性の改善が難しいという課題も解決できる。

　なお上記では、周波数分離部１１１は力の指令値を周波数分離し、周波数分離部１２２はトルクの現在値を周波数分離する場合を示した。一方、例えばトルク指令値変換部１１２及び力演算部１１３のように、トルクと力は相互に変換が可能である。そのため、周波数分離部１１１は、力の指令値をトルクの指令値に変換した後に、周波数分離を行ってもよい。同様に、周波数分離部１２２は、トルクの現在値を力の現在値に変換した後に、周波数分離を行ってもよい。別言すれば、力とトルクの相互変換と、周波数分離部１１１，１２２による演算を入替えたとしても、周波数分離部１１１が力の指令値を周波数分離したり、周波数分離部１２２がトルクの現在値を周波数分離したりするのと同等となる。また、周波数分離部１１１にトルクの指令値が入力されてもよいし、周波数分離部１２２に力の現在値が入力されてもよい。

　また、力の指令値とトルクの現在値を別々に周波数分離する代わりに、力の指令値からトルクの現在値を差し引いた偏差を周波数分離してもよい。これは、周波数分離部１１１と周波数分離部１２２を１つに統合したものと解釈でき、同等となる。

　また上記では、周波数分離部１１１がメイン制御部１１の内部に設けられた場合を示した。しかしながら、これに限らず、周波数分離部１１１がメイン制御部１１の外部に設けられていてもよい。
　また上記では、周波数分離部１２２が関節制御部１２の内部に設けられた場合を示した。しかしながら、これに限らず、周波数分離部１２２が関節制御部１２の外部に設けられていてもよい。
　図８では、周波数分離部１１１及び周波数分離部１２２が、メイン制御部１１及び関節制御部１２の外部に設けられた場合を示している。

　以上のように、この実施の形態１によれば、ロボット制御装置１は、ロボット２の力の指令値又は当該ロボット２が有する関節毎のトルクの指令値を低周波成分及び高周波成分に分離する周波数分離部１１１と、ロボット２の力の現在値又は当該ロボット２が有する関節毎のトルクの現在値を低周波成分及び高周波成分に分離する周波数分離部１２２と、周波数分離部１１１により得られた高周波成分に基づいてロボット２が有する関節毎のトルクの指令値の高周波成分を演算し、周波数分離部１１１により得られた低周波成分及び周波数分離部１２２により得られた低周波成分に基づいて力制御の指令値を演算し、当該力制御の指令値に基づいてロボット２が有する関節毎の制御指令値を演算するメイン制御部１１と、ロボット２が有する関節毎に設けられ、周波数分離部１２２により得られた高周波成分及びメイン制御部１１により演算されたトルクの指令値の高周波成分に基づいてトルク制御の指令値を演算し、当該トルク制御の指令値及び当該メイン制御部１１により演算された制御指令値に基づいて対応する関節に設けられたモータ２１に対する指令値を演算する関節制御部１２とを備え、周波数分離部１１１及び周波数分離部１２２は、メイン制御部１１及び関節制御部１２の外部又は内部に設けられた。これにより、実施の形態１に係るロボット制御装置１は、従来構成に対して力制御の性能を向上可能となる。また、実施の形態１に係るロボット制御装置１は、制御偏差を抑制可能となる。

実施の形態２．
　実施の形態１では、関節制御部１２において、メイン制御部１１で演算された角速度の指令値を用いて角速度制御の指令値を演算し、その後、トルク制御の指令値及び角速度制御の指令値を合成する場合を示した。しかしながら、これに限らず、関節制御部１２において、トルク制御の指令値及びメイン制御部１１で演算された角速度の指令値を合成した後、その合成結果を用いて角速度制御の指令値を演算してもよい。
　図９，１０は実施の形態２に係るロボット制御装置１の構成例を示す図である。図９，１０に示す実施の形態２に係るロボット制御装置１は、図１，２に示す実施の形態１に係るロボット制御装置１に対し、関節角制御部１２５及び指令値合成部１２６を指令値合成部１２７及び関節角制御部１２８に変更している。その他の構成は同様であり、同一の符号を付してその説明を省略する。

　指令値合成部１２７は、メイン制御部１１により演算された角速度の指令値及びトルク制御部１２３により演算されたトルク制御の指令値を合成する。図１０では、指令値合成部１２７は、加算器１２７１を有する。加算器１２７１は、メイン制御部１１により演算された角速度の指令値とトルク制御部１２３により演算されたトルク制御の指令値とを加算する。

　関節角制御部１２８は、指令値合成部１２７による合成結果に基づいて、角速度制御の指令値を演算する。関節角制御部１２８は、速度変換部１２８１及び速度制御部１２８２を有している。速度制御部１２８２は、減算器１２８３及びＰＩ制御部１２８４を有している。

　速度変換部１２８１は、対応する関節での角度の現在値を角速度の現在値に変換する。

　減算器１２８３は、指令値合成部１２７による合成結果から、速度変換部１２８１により得られた角速度の現在値を減算する。
　ＰＩ制御部１２８４は、減算器１２８３による減算結果に基づいてＰＩ制御を行うことで、角速度制御の指令値を得る。
　この関節角制御部１２８により演算された角速度制御の指令値（電流指令値）は、対応する関節に設けられたモータ２１に出力される。

　このように、実施の形態２に係るロボット制御装置１では、角速度の指令値及びトルク制御の指令値を合成し、その合成結果に基づいて角速度制御を実施する。この実施の形態２に係るロボット制御装置１についても、実施の形態１に係るロボット制御装置１と同様の効果が得られる。また、実施の形態２に係るロボット制御装置１は、従来のコンプライアンス制御に近い対応関係となる。

実施の形態３．
　図１，２に示す実施の形態１に係るロボット制御装置１では、周波数分離部１２２が関節制御部１２に設けられた場合を示した。しかしながら、これに限らず、周波数分離部１２２は、メイン制御部１１と関節制御部１２とに分割されて設けられていてもよい。
　図１１は実施の形態３に係るロボット制御装置１の構成例を示す図である。図１１に示す実施の形態３に係るロボット制御装置１は、図１，２に示す実施の形態１に係るロボット制御装置１に対し、周波数分離部１２２をローパスフィルタ１１９、加算部１２０及び減算部１２９に変更している。ローパスフィルタ１１９、加算部１２０及び減算部１２９は、ロボット２が有する関節毎に設けられている。図１１，１２に示すように、ローパスフィルタ１１９、加算部１２０及び減算部１２９は、周波数分離部（第２周波数分離部）１３を構成する。その他の構成は同様であり、同一の符号を付してその説明を省略する。なお、図１２において、（低）は低周波成分を示し、（高）は高周波成分を示し、（低＋高）は両方の成分、すなわち元の信号であることを示している。

　ローパスフィルタ１１９は、メイン制御部１１に設けられ、トルク取得部１２１により取得されたトルクの現在値の低周波成分のみを通す。
　加算部１２０は、メイン制御部１１に設けられ、ローパスフィルタ１１９を通過したトルクの現在値の低周波成分と、トルク指令値変換部１１２により得られたトルクの指令値の高周波成分とを加算する。

　減算部１２９は、関節制御部１２に設けられ、加算部１２０による加算結果から、トルク取得部１２１により取得されたトルクの現在値を減算する。減算部１２９による出力は、トルクの指令値の高周波成分からトルクの現在値の高周波成分を減算した値となる。

　なお、実施の形態３に係るロボット制御装置１による位置姿勢の制御は、実施の形態１に係るロボット制御装置１による位置姿勢の制御と同様である。実施の形態３に係るロボット制御装置１による力制御（低周波制御）は、周波数分離部１３で得られたトルクの現在値の低周波成分を力演算部１１３で力の現在値の低周波成分に変換したものを用いて行っており、実施の形態１に係るロボット制御装置１による力制御と実質的に同一である。一方、実施の形態３に係るロボット制御装置１によるトルク制御（高周波制御）は、実施の形態１に係るロボット制御装置１によるトルク制御とは異なる。以下では、このトルク制御に関する部分についてのみ説明を行う。

　実施の形態３に係るロボット制御装置１では、トルク取得部１２１により取得されたトルクの現在値は関節制御部１２で分岐された後、一方はメイン制御部１１へと出力される。メイン制御部１１へと出力されたトルクの現在値は、ローパスフィルタ１１９に入力され、トルクの現在値の低周波成分が得られる。この信号は、力演算部１１３と加算部１２０に出力される。加算部１２０は、トルク指令値変換部１１２により得られたトルクの指令値の高周波成分とトルクの現在値の低周波成分を加算して、関節制御部１２へと出力する。関節制御部１２へと出力された上記加算値は、減算部１２９に入力され、トルクの現在値が減算される。その結果、メイン制御部１１からのトルクの現在値の低周波成分と、トルク取得部１２１により取得されたトルクの現在値の低周波成分とが相殺されるため、トルクの現在値の高周波成分が残る。このトルクの現在値の高周波成分は、トルクの指令値の高周波成分から減算され、周波数分離部１３からはトルクの偏差（指令値と現在値との差）の高周波成分が出力される。このトルクの偏差の高周波成分は、トルク制御部１２３に出力される。以降は、実施の形態１と同様である。

　この実施の形態３に係るロボット制御装置１では、関節制御部１２内にフィルタ処理等を行う構成が存在しないため、トルク制御機能を有する関節制御部１２であれば変更なくそのまま利用できるという利点がある。

　なお、減算部１２９が加算部１２０から得るトルクの現在値の低周波成分は、周波数分離部１３に入力されたトルクの現在値から見ると２回の通信を経て得られる値であるため、遅延が存在する。よって、厳密には、低周波成分が完全に相殺されるわけではない。しかしながら、低周波成分は元々変化がゆっくりであり、遅延がローパスフィルタ１１９の時定数と比較して十分に小さければ遅延の影響は小さくなる。

　また図１１，１２では、ローパスフィルタ１１９を用いた場合を示している。しかしながら、これに限らず、カルマンフィルタ又はオブザーバを用いてトルクの現在値の低周波成分を推定してもよい。

　なお、本願発明はその発明の範囲内において、各実施の形態の自由な組合わせ、或いは各実施の形態の任意の構成要素の変形、若しくは各実施の形態において任意の構成要素の省略が可能である。例えば、実施の形態１－３では、メイン制御部が角速度の指令値を演算し、関節角制御部が速度制御を行う例を用いて説明したが、メイン制御部が加速度又は電流等の他の物理量の指令値を演算し、関節角制御部がそれらの物理量の制御を行うことで位置姿勢と力の制御を行うことも可能である。

　この発明に係るロボット制御装置は、従来構成に対して力制御の性能を向上可能となり、ロボットの位置姿勢と力を同時に制御可能なロボット制御装置等に用いるのに適している。

１　ロボット制御装置
２　ロボット
１１　メイン制御部
１２　関節制御部
１３　周波数分離部（第２周波数分離部）
２１　モータ
２２　センサ
２３　トルクセンサ
２４　エンコーダ
１１１　周波数分離部（第１周波数分離部）
１１２　トルク指令値変換部
１１３　力演算部
１１４　力制御部
１１５　位置姿勢演算部
１１６　位置姿勢制御部
１１７　指令値合成部
１１８　指令値変換部
１１９　ローパスフィルタ
１２０　加算部
１２１　トルク取得部
１２２　周波数分離部（第２周波数分離部）
１２３　トルク制御部
１２４　モータ制御部
１２５　関節角制御部
１２６　指令値合成部
１２７　指令値合成部
１２８　関節角制御部
１２９　減算部
１１１１　ローパスフィルタ
１１１２　ハイパスフィルタ
１１１３　ローパスフィルタ
１１１４　減算器
１１２１　係数乗算部
１１３１　係数乗算部
１１４１　減算器
１１４２　係数乗算部
１１６１　偏差演算器
１１６２　係数乗算部
１１７１　加算器
１１８１　係数乗算部
１２３１　減算器
１２３２　ＰＩ制御部
１２５１　速度変換部
１２５２　速度制御部
１２５３　減算器
１２５４　ＰＩ制御部
１２６１　加算器
１２７１　加算器
１２８１　速度変換部
１２８２　速度制御部
１２８３　減算器
１２８４　ＰＩ制御部

Claims

　ロボットの力の指令値又は当該ロボットが有する関節毎のトルクの指令値を低周波成分及び高周波成分に分離する第１周波数分離部と、
　前記ロボットの力の現在値又は当該ロボットが有する関節毎のトルクの現在値を低周波成分及び高周波成分に分離する第２周波数分離部と、
　前記第１周波数分離部により得られた高周波成分に基づいて前記ロボットが有する関節毎のトルクの指令値の高周波成分を演算し、前記第１周波数分離部により得られた低周波成分及び前記第２周波数分離部により得られた低周波成分に基づいて力制御の指令値を演算し、当該力制御の指令値に基づいて前記ロボットが有する関節毎の制御指令値を演算するメイン制御部と、
　前記ロボットが有する関節毎に設けられ、前記第２周波数分離部により得られた高周波成分及び前記メイン制御部により演算されたトルクの指令値の高周波成分に基づいてトルク制御の指令値を演算し、当該トルク制御の指令値及び当該メイン制御部により演算された制御指令値に基づいて対応する関節に設けられたモータに対する指令値を演算する関節制御部とを備え、
　前記第１周波数分離部及び前記第２周波数分離部は、前記メイン制御部及び前記関節制御部の外部又は内部に設けられた
　ことを特徴とするロボット制御装置。
　前記関節制御部は、
　トルク制御の指令値及び前記メイン制御部により演算された関節毎の制御指令値を合成することで、前記モータに対する指令値を得る指令値合成部を備えた
　ことを特徴とする請求項１記載のロボット制御装置。
　前記メイン制御部は、前記制御指令値として、前記力制御の指令値に基づいて前記ロボットが有する関節毎の角速度の指令値を演算し、
　前記関節制御部は、
　前記メイン制御部により演算された関節毎の角速度の指令値に基づいて、角速度制御の指令値を演算する関節角制御部と、
　トルク制御の指令値及び前記関節角制御部により演算された角速度制御の指令値を合成することで、前記モータに対する指令値を得る指令値合成部とを備えた
　ことを特徴とする請求項１記載のロボット制御装置。
　前記メイン制御部は、前記制御指令値として、前記力制御の指令値に基づいて前記ロボットが有する関節毎の角速度の指令値を演算し、
　前記関節制御部は、
　トルク制御の指令値及び前記メイン制御部により演算された関節毎の角速度の指令値を合成する指令値合成部と、
　前記指令値合成部による合成結果に基づいて角速度制御の指令値を演算することで、前記モータに対する指令値を得る関節角制御部とを備えた
　ことを特徴とする請求項１記載のロボット制御装置。
　前記第２周波数分離部は、
　トルクの現在値から低周波成分を得るローパスフィルタと、
　前記ローパスフィルタにより得られた低周波成分と、前記メイン制御部により演算されたトルクの指令値の高周波成分とを加算する加算部と、
　前記加算部により得られた成分からトルクの現在値を減算することで高周波成分を得る減算部とを備え、
　前記ローパスフィルタ及び前記加算部は前記メイン制御部に設けられ、
　前記減算部は前記関節制御部に設けられた
　ことを特徴とする請求項１から請求項４のうちの何れか１項記載のロボット制御装置。
　第１周波数分離部と、第２周波数分離部と、メイン制御部と、ロボットが有する関節毎に設けられた関節制御部とを備え、前記第１周波数分離部及び前記第２周波数分離部は、前記メイン制御部及び前記関節制御部の外部又は内部に設けられたロボット制御装置によるロボット制御方法であって、
　前記第１周波数分離部は、前記ロボットの力の指令値又は当該ロボットが有する関節毎のトルクの指令値を低周波成分及び高周波成分に分離し、
　前記第２周波数分離部は、前記ロボットの力の現在値又は当該ロボットが有する関節毎のトルクの現在値を低周波成分及び高周波成分に分離し、
　前記メイン制御部は、前記第１周波数分離部により得られた高周波成分に基づいて前記ロボットが有する関節毎のトルクの指令値の高周波成分を演算し、前記第１周波数分離部により得られた低周波成分及び前記第２周波数分離部により得られた低周波成分に基づいて力制御の指令値を演算し、当該力制御の指令値に基づいて前記ロボットが有する関節毎の制御指令値を演算し、
　前記関節制御部は、前記第２周波数分離部により得られた高周波成分及び前記メイン制御部により演算されたトルクの指令値の高周波成分に基づいてトルク制御の指令値を演算し、当該トルク制御の指令値及び当該メイン制御部により演算された制御指令値に基づいて対応する関節に設けられたモータに対する指令値を演算する
　ことを特徴とするロボット制御方法。