ロボット制御装置
【課題】摩擦力トルクを高精度に推定し、力覚センサを使用しない場合においても、より高精度にロボットに作用する外力を推定することができ、安定した力制御を実現することを可能にする。
【解決手段】一実施形態では、ロボットに作用する外力により発生した外乱トルクをより高精度に算出するため、作業形態や動作速度などに基づいて摩擦係数や不感帯閾値などのアルゴリズムに含まれる摩擦パラメータを動的に変更させて、駆動トルクを高精度で推定する。
【解決手段】一実施形態では、ロボットに作用する外力により発生した外乱トルクをより高精度に算出するため、作業形態や動作速度などに基づいて摩擦係数や不感帯閾値などのアルゴリズムに含まれる摩擦パラメータを動的に変更させて、駆動トルクを高精度で推定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明の実施形態は、モータなどのアクチュエータを用いてロボットを駆動するロボット制御装置に関する。
【背景技術】
【０００２】
ロボットの先端部（例えば、アーム先端部など）が対象物と接触しながら、作業座標系での位置および接触力を制御する、いわゆる力制御では、先端部において作用している外力を推定し、これに応じた動作の修正を柔軟に行うことが重要である。先端部に力覚センサを取り付けた場合、外力を高精度に測定できるが、力覚センサが高価である上に衝撃に対して脆弱であるため、センサの使用が敬遠されることが多い。センサを使用しない場合においては、ロボットに作用している外力の影響で発生するアクチュエータの外乱トルクを推定する必要がある。外乱トルクは、実際の駆動トルク指令値から、アクチュエータの運動に必要な推定駆動トルクを引くことで算出される。そのため算出に使用する動力学モデルのパラメータを高精度に同定する必要がある。
【０００３】
特に摩擦力は、アクチュエータ速度、その動作方向、動作履歴、また先端に取り付けられた負荷の違いといった短期的な要因や、季節や昼夜による温度変化、使用状況による経年劣化などの長期的な要因で、複雑に変化することが知られている。摩擦力の近似モデルに含まれるパラメータは上記要因によって変化していくため、適切なパラメータをリアルタイムで同定することが求められる。クーロン摩擦係数と、粘性摩擦係数と、一定外乱トルクとをリアルタイムで同定する技術が知られている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００６−２０４８７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
クーロン摩擦係数と、粘性摩擦係数と、一定外乱トルクとをリアルタイムで同定する技術は、粘性摩擦を折れ線で近似しているが、実際には駆動速度が上がるにつれて摩擦力トルクは飽和していくことが知られている。より広い範囲の速度において高精度な近似を行うためには逆正接関数や、複数の指数関数の線形和のようなモデルが提案されており、こうしたモデルに対してもリアルタイムで同定することが求められる。
【０００６】
そこで、本発明の一実施形態では、摩擦力トルクを高精度に推定し、力覚センサを使用しない場合においても、より高精度にロボットに作用する外力を推定することができ、安定した力制御を実現することができるロボット制御装置を提供する。
【課題を解決するための手段】
【０００７】
本実施形態によれば、関節軸および前記関節軸に駆動力を伝達する駆動軸を有するロボットを制御するロボット制御装置であって、前記関節軸を制御周期毎に駆動するアクチュエータと、前記駆動軸の角度を検出する駆動軸角度検出部と、前記駆動軸の角度から前記関節軸の角度を算出する関節軸角度算出部と、前記関節軸の角度から前記ロボットの先端位置を算出する先端位置算出部と、前記先端位置の位置指令値と、前記先端位置との位置誤差を算出する位置誤差算出部と、前記関節軸の角度に基づいて前記先端位置での作業座標系と関節座標系との間のヤコビ行列を算出するヤコビ行列算出部と、関節角差分を算出する関節角差分算出部と、前記関節角差分に基づいて関節のトルク指令値を算出するトルク指令値算出部と、前記トルク指令値に基づいて前記アクチュエータを駆動する駆動部と、前記関節軸の角度から前記アクチュエータを駆動するための駆動トルクを推定する駆動トルク推定部と、推定された前記駆動トルクと前記トルク指令値との差を外部トルクとして算出する外部トルク算出部と、前記外部トルクと前記ヤコビ行列から、前記先端位置に作用する外力を算出する外力算出部と、前記先端位置におけるコンプライアンスモデルを記憶するコンプライアンスモデル記憶部と、前記コンプライアンスモデルを用いて、前記外力に応じて前記先端位置での作業座標系位置に対するコンプライアンス修正量を算出し、前記コンプライアンス修正量を用いて前記位置誤差を修正するコンプライアンス修正量算出部と、摩擦力トルクを近似するモデルの、少なくとも１つのパラメータを有する摩擦パラメータを記憶する摩擦パラメータ記憶部と、摩擦力トルク評価部と、を備え、前記駆動トルク推定部は、前記駆動軸の角度から前記駆動軸の速度を算出する駆動軸速度算出部と、前記関節軸に作用する慣性力トルクを算出する慣性力トルク算出部と、前記関節軸に作用する重力を算出する重力トルク算出部と、算出された前記駆動軸の速度と、前記摩擦パラメータ記憶部に記憶された前記摩擦パラメータとを用いて第１の摩擦力トルクを推定する第１の摩擦力トルク算出部と、前記摩擦パラメータを変化させて複数個の第２の摩擦力トルクを算出する第２の摩擦力トルク算出部と、を有し、前記摩擦力トルク評価部は、算出された第１の摩擦力トルクと、複数個の第２の摩擦力トルクから選択された一つの第２の摩擦力トルクと、算出された慣性力トルクと、算出された重力トルクとの和を算出し、この和と前記トルク指令値算出部に算出された前記トルク指令値との差の演算を、前記複数個の第２の摩擦力トルクのそれぞれに対して行い、前記差の絶対値が最小となる摩擦パラメータを選択し、この選択結果に基づいて、前記摩擦パラメータが更新されることを特徴とするロボット制御装置が提供される。
【図面の簡単な説明】
【０００８】
【図１】一実施形態によるロボット制御装置の制御ブロック図。
【図２】一実施形態に係る駆動トルク推定部の一具体例を示す制御ブロック図。
【図３】摩擦力τｆと、駆動軸速度ωとの関係を示すグラフ。
【図４】摩擦パラメータの集合を説明する図。
【図５】一実施形態によるロボット制御装置の動作を示すフローチャート。
【図６】摩擦パラメータ更新処理の動作を示すフローチャート。
【図７】第２変形例の制御ブロック図。
【発明を実施するための形態】
【０００９】
実施形態を以下に図面を参照して説明する。以下の図面の記載において、同一又は類似の部分には同一又は類似の符号を付している。ただし、図面は模式的なものであり、具体的な寸法は以下の説明を参酌して判断すべきものである。又、図面相互間においても互いの寸法の関係や比率が異なる部分が含まれていることはもちろんである。
【００１０】
一実施形態によるロボット制御装置は、関節軸および前記関節軸に駆動力を伝達する駆動軸を有するロボットを制御するロボット制御装置であって、前記関節軸を制御周期毎に駆動するアクチュエータと、前記駆動軸の角度を検出する駆動軸角度検出部と、前記駆動軸の角度から前記関節軸の角度を算出する関節軸角度算出部と、前記関節軸の角度から前記ロボットの先端位置を算出する先端位置算出部と、前記先端位置の位置指令値と、前記先端位置との位置誤差を算出する位置誤差算出部と、前記関節軸の角度に基づいて前記先端位置での作業座標系と関節座標系との間のヤコビ行列を算出するヤコビ行列算出部と、関節角差分を算出する関節角差分算出部と、前記関節角差分に基づいて関節のトルク指令値を算出するトルク指令値算出部と、前記トルク指令値に基づいて前記アクチュエータを駆動する駆動部と、前記関節軸の角度から前記アクチュエータを駆動するための駆動トルクを推定する駆動トルク推定部と、推定された前記駆動トルクと前記トルク指令値との差を外部トルクとして算出する外部トルク算出部と、前記外部トルクと前記ヤコビ行列から、前記先端位置に作用する外力を算出する外力算出部と、前記先端位置におけるコンプライアンスモデルを記憶するコンプライアンスモデル記憶部と、前記コンプライアンスモデルを用いて、前記外力に応じて前記先端位置での作業座標系位置に対するコンプライアンス修正量を算出し、前記コンプライアンス修正量を用いて前記位置誤差を修正するコンプライアンス修正量算出部と、摩擦力トルクを近似するモデルの、少なくとも１つのパラメータを有する摩擦パラメータを記憶する摩擦パラメータ記憶部と、摩擦力トルク評価部と、を備え、前記駆動トルク推定部は、前記駆動軸の角度から前記駆動軸の速度を算出する駆動軸速度算出部と、前記関節軸に作用する慣性力トルクを算出する慣性力トルク算出部と、前記関節軸に作用する重力を算出する重力トルク算出部と、算出された前記駆動軸の速度と、前記摩擦パラメータ記憶部に記憶された前記摩擦パラメータとを用いて第１の摩擦力トルクを推定する第１の摩擦力トルク算出部と、前記摩擦パラメータを変化させて複数個の第２の摩擦力トルクを算出する第２の摩擦力トルク算出部と、を有し、前記摩擦力トルク評価部は、算出された第１の摩擦力トルクと、複数個の第２の摩擦力トルクから選択された一つの第２の摩擦力トルクと、算出された慣性力トルクと、算出された重力トルクとの和を算出し、この和と前記トルク指令値算出部に算出された前記トルク指令値との差の演算を、前記複数個の第２の摩擦力トルクのそれぞれに対して行い、前記差の絶対値が最小となる摩擦パラメータを選択し、この選択結果に基づいて、前記摩擦パラメータが更新されることを特徴とする。
【００１１】
一実施形態によるロボット制御装置は、図１に示すように、中央演算処理装置（ＣＰＵ）１と、アクチュエータ１００と、駆動部（アンプ）１０１と、駆動軸角度検出部１０２と、位置データ記憶部２００と、リンクパラメータ記憶部２０１と、摩擦パラメータ記憶部２０２と、ヤコビ行列記憶部２０３と、力／モーメントデータ記憶部２０４と、コンプライアンスモデル記憶部２０５と、を備えている。
【００１２】
駆動軸角度検出部１０２は、エンコーダ等の位置センサが使用可能であり、所定の周波数成分を除去するフィルタを備えていても良い。駆動軸角度検出部１０２は、駆動軸の位置の変位量（駆動軸の角度）を検出する。
【００１３】
図１に示したＣＰＵ１は、関節軸角度算出部１０３と、先端位置算出部１０４と、ヤコビ行列算出部１０５と、駆動トルク推定部１０６と、外部トルク算出部１０７と、外力算出部１０８と、力指令値生成部１０９と、力誤差算出部１１０と、コンプライアンス修正量算出部１１１と、位置指令値生成部１１２と、位置誤差算出部１１３と、関節角差分算出部１１４と、トルク指令値算出部１１５と、をハードウェア資源であるモジュール（論理回路）として論理的に備えている。
【００１４】
関節軸角度算出部１０３は、駆動軸角度検出部１０２により算出された駆動軸角度に基づいて、減速機の減速比と、伝達機構のもつ駆動軸と関節軸の比とに応じた関節軸角度を算出する。なお、関節軸に取り付けられる関節軸角度検出部（図示せず）により関節軸角度を直接求めてもよい。
【００１５】
先端位置算出部１０４は、リンクパラメータ記憶部２０１からリンクパラメータを読み出し、関節軸角度算出部１０３により算出された関節軸角度と、読み出されたリンクパラメータとを用いて、順運動学計算によりロボットの作業座標系における先端部の位置、すなわち先端位置を算出する。
【００１６】
ヤコビ行列算出部１０５は、関節軸角度算出部１０３により算出された関節軸角度から、ヤコビ行列を算出し、算出されたヤコビ行列をヤコビ行列記憶部２０３に格納する。ヤコビ行列とは、ロボットの作業座標系と関節座標系との間の微小変位関係を表現した行列である。ヤコビ行列をＪとすると、ロボットの先端位置の誤差Δｘと、関節角差分Δθは式（１）の関係を満たしている。
Δｘ＝ＪΔθ … （１）
【００１７】
駆動トルク推定部１０６は、駆動軸角度検出部１０２により検出された駆動軸角度と、関節軸角度算出部１０３により算出された関節軸角度とを用いて、ロボットの関節軸を駆動するのに必要な駆動トルクを推定する。この駆動トルク推定部１０６の一具体例を図２に示す。この具体例の駆動トルク推定部１０６は、図２に示すように、駆動軸速度算出部３００と、第１の摩擦力トルク算出部３０１と、慣性力トルク算出部３０２と、重力トルク算出部３０３と、加算部３０４と、第２の摩擦力トルク算出部３０５と、を備えている。
【００１８】
図２に示すように、駆動軸速度算出部３００は、駆動軸角度算出部１０２により算出された駆動軸角度に基づいて、例えば駆動軸角度の所定時間毎の差分を取ることにより駆動軸速度を算出する。第１の摩擦力トルク算出部３０１は、摩擦パラメータ記憶部２０２に格納された摩擦パラメータを読み出して、この読み出された摩擦パラメータと、駆動軸速度算出部３００により算出された駆動軸速度と、を用いてクーロン摩擦、粘性摩擦等に相当する第１の摩擦力トルクを算出する。
【００１９】
第２の摩擦力トルク算出部３０５について説明する。摩擦パラメータ記憶部２０２に格納された摩擦パラメータと、微小変化パラメータ記憶部３０６に格納された微小変化分とから、複数の摩擦パラメータの集合を用意する。例えば摩擦力τｆが駆動軸速度ω（＝ｄθ／ｄｔ）に対して、次の式（２）で表される関数
τf＝Ａ×ａｒｃｔａｎ（Ｂ×ω）＋ Ｃ …（２）
で定義されているとすると、摩擦パラメータは３種類のパラメータＡ、Ｂ、Ｃの組（Ａ、Ｂ、Ｃ）である。式（２）に示す関数をグラフで表すと図３に示すようになる。それぞれのパラメータに対して微小変化分ΔＡ、ΔＢ、ΔＣからＡ＋ΔＡ、Ａ―ΔＡ、Ｂ＋ΔＢ、Ｂ―ΔＢ、Ｃ＋ΔＣ、Ｃ―ΔＣを算出し、それらを組み合わせた摩擦パラメータの集合に対して第２の摩擦力トルクを算出する。この場合、摩擦パラメータの集合は、図４からわかるように、微小変化しないパラメータを含めると、２７個となる。この２７個の摩擦パラメータの集合のうち、パラメータＡ、Ｂ、Ｃが全く変化しない摩擦パラメータ（Ａ、Ｂ、Ｃ）に対する摩擦力は第１の摩擦力トルク算出部３０１により算出されたものである。そして、残りの２６通りの摩擦パラメータに対して、例えば式（２）を用いて、第２の摩擦力トルク算出部３０５によって第２の摩擦力トルクを算出する。したがって、第２の摩擦力トルクは、２６個の値を有する。
【００２０】
慣性力トルク算出部３０２は、関節軸角度算出部１０３により算出された関節軸角度を用いて関節角加速度を算出し、リンクパラメータ記憶部２０１からリンクパラメータを読み出し、この読み出されたリンクパラメータと、関節軸角度算出部１０３により算出された関節軸角度とを用いて各リンクの慣性モーメントを算出し、算出された関節角加速度と、算出された慣性モーメントとを用いて慣性力トルクを算出する。
【００２１】
重力トルク算出部３０３は、リンクパラメータ記憶部２０１からリンクパラメータを読み出し、この読み出されたリンクパラメータと、関節軸角度算出部１０３により算出された関節軸角度とを用いて、各リンクに作用する重力を算出し、この算出された重力を補償する重力トルクを算出する。
【００２２】
加算部３０４は、第１の摩擦力トルク算出部３０１により算出された第１の摩擦力トルクと、慣性力トルク算出ユニット３０２により算出された慣性力トルクと、重力トルク算出部３０３により算出された重力トルクとを加算し、それらの和を、推定した駆動トルクとして出力する。
【００２３】
このようにして駆動トルク推定部１０６により推定された駆動トルクと、トルク指令値算出部１１５により算出されたトルク指令値との差が、外部トルクとして図１に示した外部トルク算出部１０７によって算出される。
【００２４】
摩擦力トルク評価部３０７は、算出された第１の摩擦力トルクと、２６個の第２の摩擦力トルクから選択された一つの第２の摩擦力トルクと、算出された慣性力トルクと、算出された重力トルクとを加算し、これらの和とトルク指令値との差を演算する。この差の演算を、２６個の第２の摩擦力トルクに対して行う。そして、その差の絶対値が最小となる摩擦パラメータを摩擦パラメータの集合から選択して投票する。
【００２５】
パラメータ更新部３０８は、摩擦力トルク評価部３０７によって投票結果を摩擦パラメータ推定期間において集計し、予め定められた更新周期において最多票を集めた摩擦パラメータを摩擦パラメータ記憶部２０２へ格納することで、摩擦パラメータを更新させる。
【００２６】
図１に示す外力算出部１０８は、外部トルク算出部１０７により算出された外部トルクと、ヤコビ行列算出部１０５により算出されたヤコビ行列と、を用いて外力を算出する。仮想仕事の原理より、次の式（３）に示すように外部トルクτｄにヤコビ行列Ｊの転置行列ＪＴの逆行列を乗じることにより外力ｆｄが算出される。
ｆｄ＝（ＪＴ）−１τｄ …（３）
【００２７】
図１に示す力指令値生成部１０９は、力／モーメントデータ記憶部２０４に記憶された力／モーメントデータを読み出して、各制御周期における力／モーメント指令値を出力する。
【００２８】
力誤差算出部１１０は、力指令値生成部１０９で算出された力／モーメント指令値と、外力算出部１０８で算出された外力とに基づいて力誤差Δｆを算出する。
【００２９】
コンプライアンス修正量算出部１１１は、コンプライアンスモデル記憶部２０５からコンプライアンスモデルを読み出して、読み出されたコンプライアンスモデルを用いて、力誤差算出部１１０の出力Δｆに応じた位置修正量を算出する。ここで、コンプライアンスモデルとは、例えば式（４）に示すように接触対象との間において仮想的に慣性、粘性、剛性を想定したものである。
Ｍｄ２Δｘ／ｄｔ２＋ＤｄΔｘ／ｄｔ＋ＫΔｘ＝ＫｆΔｆ …（４）
ここで、Δｘは先端部に設定された作業座標系での誤差、ｄΔｘ／ｄｔは作業座標系での速度、ｄ２Δｘ／ｄｔ２は作業座標系での加速度ベクトル、Ｍは慣性行列、Ｄは粘性係数行列、Ｋは剛性係数行列、Ｋｆは力フィードバックゲイン行列である。力と位置の作用させる軸を切り替えるコンプライアンス選択行列は力フィードバックゲイン行列Ｋｆに含まれた形になっている。誤差の速度ｄΔｘ／ｄｔ及び加速度ベクトルｄ２Δｘ／ｄｔ２は、それぞれ位置誤差ベクトルΔｘの時間に関する一回差分、二回差分で近似できることから、コンプライアンス修正量Δｘｃｏｍｐは、次の式（５）を用いて算出することができる。
Δｘｃｏｍｐ＝１／Ｋ（ＫｆΔｆ−Ｍｄ２Δｘ／ｄｔ２−ＤｄΔｘ／ｄｔ） …（５）
【００３０】
位置指令値生成部１１２は、位置データ記憶部２００に記憶された目標先端位置データを読み出して、この目標先端位置データから各制御周期における補間された先端位置指令値を算出する。
【００３１】
位置誤差算出部１１３は、位置指令値生成部１１２により生成した先端位置指令値ｘＲと、先端位置算出部１０４により算出された現在の先端位置ｘと、コンプライアンス修正量算出部１１１により算出されたコンプライアンス修正量Δｘｃｏｍｐに基づいて、次の式（６）を用いて位置誤差Δｘを算出する。
Δｘ＝ｘＲ−ｘ＋Δｘｃｏｍｐ …（６）
【００３２】
関節角差分算出部１１４は、位置誤差算出部１１３から算出された位置誤差Δｘと、ヤコビ行列Ｊの逆行列Ｊ−１を用いて関節角差分Δθを、次の式（７）を用いて算出する。
Δθ＝Ｊ−１Δｘ …（７）
【００３３】
トルク指令値算出部１１５は、関節角差分算出部１１４により算出された関節角差分を積分することによりトルク指令値（制御目標値）を生成する。
【００３４】
駆動部１０１は、トルク指令値算出部１１５により算出されたトルク指令値にしたがってアクチュエータ１００を制御周期毎に駆動する。
【００３５】
位置データ記憶部２００、リンクパラメータ記憶部２０１、摩擦パラメータ記憶部２０２、ヤコビ行列記憶部２０３、力／モーメントデータ記憶部２０４、及びコンプライアンスモデル記憶部２０５としては、例えば半導体メモリ、磁気ディスク、光ディスク、光磁気ディスクや磁気テープ等が採用可能である。
【００３６】
位置データ記憶部２００は、位置指令値生成部１１１が先端位置指令値を生成するために使用する目標先端位置データ列を記憶している。リンクパラメータ記憶部２０１は、ロボットのリンクに関するリンクパラメータを記憶している。摩擦パラメータ記憶部２０２は、第１の摩擦力トルク算出部３０１が第１の摩擦力トルクを算出するために使用する、予め定速運転において速度−トルク関係から求めておいた摩擦パラメータのデータを記憶している。ヤコビ行列記憶部２０３は、ヤコビ行列算出部１０５により算出されるヤコビ行列を記憶している。力／モーメントデータ記憶部２０４は、力指令値生成部１０９が先端位置力指令値を生成するために使用する目標先端力データ列を記憶している。コンプライアンスモデル記憶部２０５は、予め設定されたコンプライアンスモデルを記憶している。
【００３７】
（ロボット制御方法）
次に、本実施形態によるロボット制御装置の動作を、図５に示すフローチャートを参照しながら説明する。
【００３８】
（ａ）まず、制御演算を開始し、位置指令値生成部１１２によって、位置データ記憶部２００から目標先端位置データ列が読み出されて、目標先端位置データ列に基づいて各制御周期における先端位置指令値が生成される（ステップ、Ｓ１００、Ｓ１０１）。
【００３９】
次に、ステップＳ１０２において、位置指令値生成部１１２により生成された先端位置指令値と、先端位置算出部１０４により算出された先端位置から、後述するコンプライアンス修正量を考慮した位置誤差が位置誤差算出部１１３によって算出される。続いて、ステップＳ１０３において、位置誤差算出部１１３により算出された位置誤差に対して、ヤコビ行列記憶部２０３から読み出したヤコビ行列を用いて式（７）に示すように、逆運動学計算を行って関節角差分が算出される。
【００４０】
その後、ステップＳ１０４において、関節角差分算出部１１４により算出された関節角差分を積分することによりトルク指令値がトルク指令値算出部１１５によって算出される。
【００４１】
次に、ステップＳ１０５において、トルク指令値算出部１１５により算出されたトルク指令値を制御目標値として駆動部１０１によってアクチュエータ１００が駆動されることにより、駆動軸３０６が駆動され、先端位置が制御される。
【００４２】
続いて、ステップＳ１０６において、制御演算の終了を確認し、ステップＳ１０７でサーボ処理を終了する。ステップＳ１０６において制御演算が終了でなければ後述するステップＳ１０８に進む。
【００４３】
（ｂ）次に、ステップＳ１０８において、駆動軸角度検出部１０２によって駆動軸角度を検出する。
【００４４】
続いて、ステップＳ１０９において、駆動軸角度検出部１０２により算出された駆動軸角度から、減速機の減速比などに基づいて関節軸角度算出部１０３によって関節軸角度が算出される。
【００４５】
ステップＳ１１０において、先端位置算出部１０４によって、リンクパラメータ記憶部２０１からリンクパラメータが読み出され、この読み出されたリンクパラメータと、関節軸角度算出部１０３により算出された関節軸角度とを用いて、順運動学計算により、先端位置が算出される。
【００４６】
ステップＳ１１１において、関節軸角度算出部１０３により算出された関節軸角度を用いて、ヤコビ行列算出部１０５によってヤコビ行列が算出される。
【００４７】
（ｃ）次に、ステップＳ１１２において、駆動軸角度検出部１０２により算出された駆動軸角度と、関節軸角度算出部１０３により算出された関節軸角度とを用いて駆動トルク推定部１０６によって駆動トルクが推定される。
【００４８】
ステップＳ１１３において、駆動トルク推定部１０６により推定された駆動トルクと、トルク指令値算出部１１５により算出された実際のトルク指令値との差から外部トルク算出部１０７によって外部トルクが算出される。
【００４９】
ステップＳ１１４において、外部トルク算出部１０７により算出された外部トルクと、ヤコビ行列算出部１０５により算出されたヤコビ行列とを用いて、外力算出部１０８によって式（３）に示す外力が算出される。
【００５０】
（ｄ）次に、ステップＳ１１５において、力指令値生成部１０９により力／モーメントデータ記憶部２０４から許容力／モーメントデータ列が読み出されて、この読み出された力／モーメントデータから力／モーメント指令値が算出される。
【００５１】
ステップＳ１１６において、力指令値生成部１０９により算出された力／モーメント指令値と、外力算出部１０８で算出された外力とを用いて力誤差算出部１１０によって力誤差が算出される。
【００５２】
（ｅ）次に、ステップＳ１１７において、コンプライアンス修正量算出部１１１によってコンプライアンスモデル記憶部２０５からコンプライアンスモデルが読み出され、コンプライアンスモデルを用いて式（５）に示すコンプライアンス修正量Δｘｃｏｍｐが算出され、このコンプライアンス修正量Δｘｃｏｍｐを用いて、力誤差算出部１１０により算出された力誤差に応じた先端位置修正量がコンプライアンス修正量算出部１１１によって算出される。
【００５３】
ステップＳ１０２に戻り、式（６）に示すように、位置指令値生成部１１２により生成された先端位置指令値と、先端位置算出部１０４により算出された先端位置との誤差に対して、コンプライアンス修正量算出部１１１から算出された修正量を考慮した位置誤差が位置誤差算出部１１３によって算出される。
【００５４】
駆動トルク推定処理としては、第１および第２の摩擦力トルクと、慣性力トルクと、重力トルクと用いて駆動トルクを推定する一例を示したが、これに限定されるものではない。例えば、遠心力やコリオリの力等のパラメータを更に考慮しても良い。
【００５５】
次に、本実施形態による摩擦パラメータ更新処理の動作を、図６に示すフローチャートを参照しながら説明する。なお本処理は先のロボット制御装置の制御周期と同じかもしくは数倍程度の頻度で行うことを想定するものである。
【００５６】
（ａ）摩擦パラメータ推定処理の開始するタイミング（ステップＳ２００）はステップＳ１１２における駆動トルク算出時である。ステップＳ２０１において、駆動軸角度検出部１０２により算出された駆動軸角度と、摩擦パラメータとから第１の摩擦力トルクが第１の摩擦力トルク算出部３０１によって算出される。ステップＳ２０２において、同じく駆動軸角度と、微小変化させたパラメータによる第２の摩擦力トルクが第２の摩擦力トルク算出部３０５によって算出される。
【００５７】
（ｂ）ステップＳ２０３において、外部トルク算出部１０７によって外部トルクを算出しこれを誤差とする。またステップＳ２０４において、摩擦力トルク評価部３０７によりトルク指令値および駆動トルクから誤差を算出する。
【００５８】
（ｃ）ステップＳ２０５において、それぞれの第１および第２の摩擦力トルクにおける誤差を比較し、誤差の絶対値が最小となる摩擦パラメータの集合に対して投票を行う。
【００５９】
（ｄ）ステップＳ２０６において、摩擦パラメータの更新周期であるかどうかを判定し、更新周期であればステップＳ２０７へ進んで、投票結果から最多票を得た摩擦パラメータを摩擦パラメータ記憶部２０２へ格納することで、摩擦パラメータを更新させる。摩擦パラメータの更新周期でなければステップＳ２０８へ進み、処理を終了する。
【００６０】
以上説明したように、本実施形態によれば、摩擦力トルクを高精度に推定し、力覚センサを使用しない場合においても、より高精度にロボットに作用する外力を推定することが可能となり、安定した力制御を実現することができる。
【００６１】
（第１変形例）
本実施形態の第１変形例として、摩擦力トルクの誤差の評価方法を説明する。上記実施形態では、誤差の絶対値が最小となる摩擦パラメータに投票したが、閾値を設けてトルク誤差の絶対値の最小値が上記閾値を超えない場合には、現在の摩擦パラメータに投票し、上記閾値以上となった場合には、誤差の絶対値が最小となる摩擦パラメータに投票するようにすると、摩擦パラメータの更新頻度が低くなるため、急激な変化が起こらず安定する。
【００６２】
また投票の結果、ほぼ同数の票を得た摩擦パラメータが複数存在した場合、最多得票の摩擦パラメータへと変更するのではなく、得票数に応じて微小変化分を線形補間させてもよい。例えば、
（Ａ＋ΔＡ、Ｂ―ΔＢ、Ｃ）の組がＮ1票
（Ａ＋ΔＡ、Ｂ、Ｃ＋ΔＣ）の組がＮ2票
であった場合
（Ａ＋ΔＡ、Ｂ―ΔＢ×Ｎ1／（Ｎ1＋Ｎ2）、Ｃ＋ΔＣ×Ｎ2／（Ｎ1＋Ｎ2））
とするのである。全てのケースにおいてこのような線形補間処理を行うと計算量が増加するため、得票数が同程度であった場合のみ実施する。
【００６３】
得票数が同じようになってしまう原因の一つは微小変化パラメータがやや大きいことが考えられる。したがって線形補間処理が続く場合、微小変化パラメータ記憶部３０６に格納された微小変化パラメータを変更する。
【００６４】
また、トルク指令値から慣性力トルク、重力トルク、そして摩擦パラメータにより算出した摩擦力トルクを引いた誤差には、摩擦パラメータの推定誤差に起因する誤差の他にも、ロボットに作用する外力に起因する外部トルクや、その他のパラメータの推定誤差に起因する誤差、あるいは上記実施形態において考慮していない力学的要因に起因したモデル化誤差などが含まれている。こうした外部要因を除き、摩擦パラメータの推定誤差のみに注目できるようにするために、実際にロボットを動作させて、摩擦パラメータを推定するための摩擦パラメータ推定試験を実施することも可能である。この場合は、ロボットに外力が作用しておらず、また決まった動作においてはモデル化誤差も一定となるため、誤差はパラメータ推定誤差に最も依存する。
【００６５】
一方、通常運転時においても誤差を評価することは可能であるが、上記外部要因が入り込む可能性を否定できない。そのため、誤差の評価の基準を下げてより安定した推定が行えるように配慮する。例えば第１変形例で述べたような、誤差の絶対値に対して設けた閾値を上げるとパラメータの更新頻度は低くなり、安定する。また更新周期を大きくすれば、より多くのデータが集まってからパラメータを更新するので信頼度が上がる。
【００６６】
（第２変形例）
本実施形態の第２変形例として、パラメータ変更履歴を利用したパラメータ変更について説明する。図７に示すように、摩擦パラメータ記憶部２０２内部に摩擦パラメータ変更履歴記憶部３０９と、パラメータ変更履歴データベース部３１０とを設ける。パラメータ更新部３０８によって更新された摩擦パラメータは摩擦パラメータ変更履歴記憶部３０９に格納され、摩擦力トルクを算出する際に摩擦パラメータとして利用される。摩擦パラメータの変更履歴をデータベース化したものがパラメータ変更履歴データベース部３１０へ格納される。パラメータ変更履歴データベース部３１０には同機種のロボットで計測した典型的な摩擦パラメータの変更履歴が格納されていて、微小変化パラメータ変更部３１１が両者を比較し、この比較結果に基づいて摩擦パラメータの微小変化分を修正し、微小変化パラメータ記憶部３０６へ格納する。
【００６７】
また、変更履歴には温度測定部３１２により計測された動作時の温度や、時間測定部３１３により計測された動作時刻も合わせて付加してもよい。通常、高温時や稼働時間経過とともに摩擦力は小さくなる傾向にある。温度や時刻をも合わせて記録しておくことで、もしパラメータの変更履歴がこうした一般的傾向から反している場合、故障などの異常診断にも利用できる。
【００６８】
（その他の実施形態）
上記のように、実施形態を説明したが、この開示の一部をなす論述及び図面はこの発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施の形態、実施例及び運用技術が明らかとなろう。
【００６９】
例えば、上記実施形態において、慣性力トルクの算出の際に、関節軸角度の時間に関する二回差分で算出された関節角加速度を用いているが、制御周期によっては時間遅れが大きくなりトルク誤差が大きくなりやすくなる。その場合は、リンクに搭載した加速度センサからリンクの加速度を検出し、検出した加速度を関節角加速度へ変換して、この関節角加速度を基に慣性力トルクを算出してもよい。また摩擦力のモデルとして複数の指数関数の線形和などを採用してもよく、いくつかのパラメータで特徴付けられるようなモデルに対しても本発明は適用できる。
【００７０】
また、ＣＰＵ１、位置データ記憶部２００、リンクパラメータ記憶部２０１、摩擦パラメータ記憶部２０２、ヤコビ行列記憶部２０３、力／モーメントデータ記憶部２０４、およびコンプライアンスモデル記憶部２０５等が制御対象であるロボットの内部に埋め込まれて一体化されていても良い。また、ＣＰＵ１、位置データ記憶部２００、リンクパラメータ記憶部２０１、摩擦パラメータ記憶部２０２、ヤコビ行列記憶部２０３、力／モーメントデータ記憶部２０４、およびコンプライアンスモデル記憶部２０５等が制御対象であるロボットの外部にあり、ロボットを有線或いは無線で遠隔制御することも可能である。
【００７１】
また、上記実施形態は、この発明の技術的思想を具体化するための装置や方法を例示するものであって、発明の範囲はそれらに限定されない。
【符号の説明】
【００７２】
１ 中央演算処理装置（ＣＰＵ）
１００ アクチュエータ
１０１ 駆動部
１０２ 駆動軸角度検出部
１０３ 関節軸角度算出部
１０４ 先端位置算出部
１０５ ヤコビ行列算出部
１０６ 駆動トルク推定部
１０７ 外部トルク算出部
１０８ 外力算出部
１０９ 力指令値生成部
１１０ 力誤差算出部
１１１ コンプライアンス修正量算出部
１１２ 位置指令値生成部
１１３ 位置誤差算出部
１１４ 関節角差分算出部
１１５ トルク指令値算出部
２００ 位置データ記憶部
２０１ リンクパラメータ記憶部
２０２ 摩擦パラメータ記憶部
２０３ ヤコビ行列記憶部
２０４ 力／モーメントデータ記憶部
２０５ コンプライアンスモデル記憶部
３００ 駆動軸速度算出部
３０１ 第１の摩擦力トルク算出部
３０２ 慣性力トルク算出部
３０３ 重力トルク算出部
３０４ 加算部
３０５ 第２の摩擦力トルク算出部
３０６ 微小変化パラメータ記憶部
３０７ 摩擦力トルク評価部
３０８ パラメータ更新部
３０９ パラメータ変更履歴記憶部
３１０ パラメータ変更履歴データベース部
３１１ 微小変化パラメータ変更部
３１２ 温度測定部
３１３ 時間測定部
【技術分野】
【０００１】
本発明の実施形態は、モータなどのアクチュエータを用いてロボットを駆動するロボット制御装置に関する。
【背景技術】
【０００２】
ロボットの先端部（例えば、アーム先端部など）が対象物と接触しながら、作業座標系での位置および接触力を制御する、いわゆる力制御では、先端部において作用している外力を推定し、これに応じた動作の修正を柔軟に行うことが重要である。先端部に力覚センサを取り付けた場合、外力を高精度に測定できるが、力覚センサが高価である上に衝撃に対して脆弱であるため、センサの使用が敬遠されることが多い。センサを使用しない場合においては、ロボットに作用している外力の影響で発生するアクチュエータの外乱トルクを推定する必要がある。外乱トルクは、実際の駆動トルク指令値から、アクチュエータの運動に必要な推定駆動トルクを引くことで算出される。そのため算出に使用する動力学モデルのパラメータを高精度に同定する必要がある。
【０００３】
特に摩擦力は、アクチュエータ速度、その動作方向、動作履歴、また先端に取り付けられた負荷の違いといった短期的な要因や、季節や昼夜による温度変化、使用状況による経年劣化などの長期的な要因で、複雑に変化することが知られている。摩擦力の近似モデルに含まれるパラメータは上記要因によって変化していくため、適切なパラメータをリアルタイムで同定することが求められる。クーロン摩擦係数と、粘性摩擦係数と、一定外乱トルクとをリアルタイムで同定する技術が知られている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００６−２０４８７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
クーロン摩擦係数と、粘性摩擦係数と、一定外乱トルクとをリアルタイムで同定する技術は、粘性摩擦を折れ線で近似しているが、実際には駆動速度が上がるにつれて摩擦力トルクは飽和していくことが知られている。より広い範囲の速度において高精度な近似を行うためには逆正接関数や、複数の指数関数の線形和のようなモデルが提案されており、こうしたモデルに対してもリアルタイムで同定することが求められる。
【０００６】
そこで、本発明の一実施形態では、摩擦力トルクを高精度に推定し、力覚センサを使用しない場合においても、より高精度にロボットに作用する外力を推定することができ、安定した力制御を実現することができるロボット制御装置を提供する。
【課題を解決するための手段】
【０００７】
本実施形態によれば、関節軸および前記関節軸に駆動力を伝達する駆動軸を有するロボットを制御するロボット制御装置であって、前記関節軸を制御周期毎に駆動するアクチュエータと、前記駆動軸の角度を検出する駆動軸角度検出部と、前記駆動軸の角度から前記関節軸の角度を算出する関節軸角度算出部と、前記関節軸の角度から前記ロボットの先端位置を算出する先端位置算出部と、前記先端位置の位置指令値と、前記先端位置との位置誤差を算出する位置誤差算出部と、前記関節軸の角度に基づいて前記先端位置での作業座標系と関節座標系との間のヤコビ行列を算出するヤコビ行列算出部と、関節角差分を算出する関節角差分算出部と、前記関節角差分に基づいて関節のトルク指令値を算出するトルク指令値算出部と、前記トルク指令値に基づいて前記アクチュエータを駆動する駆動部と、前記関節軸の角度から前記アクチュエータを駆動するための駆動トルクを推定する駆動トルク推定部と、推定された前記駆動トルクと前記トルク指令値との差を外部トルクとして算出する外部トルク算出部と、前記外部トルクと前記ヤコビ行列から、前記先端位置に作用する外力を算出する外力算出部と、前記先端位置におけるコンプライアンスモデルを記憶するコンプライアンスモデル記憶部と、前記コンプライアンスモデルを用いて、前記外力に応じて前記先端位置での作業座標系位置に対するコンプライアンス修正量を算出し、前記コンプライアンス修正量を用いて前記位置誤差を修正するコンプライアンス修正量算出部と、摩擦力トルクを近似するモデルの、少なくとも１つのパラメータを有する摩擦パラメータを記憶する摩擦パラメータ記憶部と、摩擦力トルク評価部と、を備え、前記駆動トルク推定部は、前記駆動軸の角度から前記駆動軸の速度を算出する駆動軸速度算出部と、前記関節軸に作用する慣性力トルクを算出する慣性力トルク算出部と、前記関節軸に作用する重力を算出する重力トルク算出部と、算出された前記駆動軸の速度と、前記摩擦パラメータ記憶部に記憶された前記摩擦パラメータとを用いて第１の摩擦力トルクを推定する第１の摩擦力トルク算出部と、前記摩擦パラメータを変化させて複数個の第２の摩擦力トルクを算出する第２の摩擦力トルク算出部と、を有し、前記摩擦力トルク評価部は、算出された第１の摩擦力トルクと、複数個の第２の摩擦力トルクから選択された一つの第２の摩擦力トルクと、算出された慣性力トルクと、算出された重力トルクとの和を算出し、この和と前記トルク指令値算出部に算出された前記トルク指令値との差の演算を、前記複数個の第２の摩擦力トルクのそれぞれに対して行い、前記差の絶対値が最小となる摩擦パラメータを選択し、この選択結果に基づいて、前記摩擦パラメータが更新されることを特徴とするロボット制御装置が提供される。
【図面の簡単な説明】
【０００８】
【図１】一実施形態によるロボット制御装置の制御ブロック図。
【図２】一実施形態に係る駆動トルク推定部の一具体例を示す制御ブロック図。
【図３】摩擦力τｆと、駆動軸速度ωとの関係を示すグラフ。
【図４】摩擦パラメータの集合を説明する図。
【図５】一実施形態によるロボット制御装置の動作を示すフローチャート。
【図６】摩擦パラメータ更新処理の動作を示すフローチャート。
【図７】第２変形例の制御ブロック図。
【発明を実施するための形態】
【０００９】
実施形態を以下に図面を参照して説明する。以下の図面の記載において、同一又は類似の部分には同一又は類似の符号を付している。ただし、図面は模式的なものであり、具体的な寸法は以下の説明を参酌して判断すべきものである。又、図面相互間においても互いの寸法の関係や比率が異なる部分が含まれていることはもちろんである。
【００１０】
一実施形態によるロボット制御装置は、関節軸および前記関節軸に駆動力を伝達する駆動軸を有するロボットを制御するロボット制御装置であって、前記関節軸を制御周期毎に駆動するアクチュエータと、前記駆動軸の角度を検出する駆動軸角度検出部と、前記駆動軸の角度から前記関節軸の角度を算出する関節軸角度算出部と、前記関節軸の角度から前記ロボットの先端位置を算出する先端位置算出部と、前記先端位置の位置指令値と、前記先端位置との位置誤差を算出する位置誤差算出部と、前記関節軸の角度に基づいて前記先端位置での作業座標系と関節座標系との間のヤコビ行列を算出するヤコビ行列算出部と、関節角差分を算出する関節角差分算出部と、前記関節角差分に基づいて関節のトルク指令値を算出するトルク指令値算出部と、前記トルク指令値に基づいて前記アクチュエータを駆動する駆動部と、前記関節軸の角度から前記アクチュエータを駆動するための駆動トルクを推定する駆動トルク推定部と、推定された前記駆動トルクと前記トルク指令値との差を外部トルクとして算出する外部トルク算出部と、前記外部トルクと前記ヤコビ行列から、前記先端位置に作用する外力を算出する外力算出部と、前記先端位置におけるコンプライアンスモデルを記憶するコンプライアンスモデル記憶部と、前記コンプライアンスモデルを用いて、前記外力に応じて前記先端位置での作業座標系位置に対するコンプライアンス修正量を算出し、前記コンプライアンス修正量を用いて前記位置誤差を修正するコンプライアンス修正量算出部と、摩擦力トルクを近似するモデルの、少なくとも１つのパラメータを有する摩擦パラメータを記憶する摩擦パラメータ記憶部と、摩擦力トルク評価部と、を備え、前記駆動トルク推定部は、前記駆動軸の角度から前記駆動軸の速度を算出する駆動軸速度算出部と、前記関節軸に作用する慣性力トルクを算出する慣性力トルク算出部と、前記関節軸に作用する重力を算出する重力トルク算出部と、算出された前記駆動軸の速度と、前記摩擦パラメータ記憶部に記憶された前記摩擦パラメータとを用いて第１の摩擦力トルクを推定する第１の摩擦力トルク算出部と、前記摩擦パラメータを変化させて複数個の第２の摩擦力トルクを算出する第２の摩擦力トルク算出部と、を有し、前記摩擦力トルク評価部は、算出された第１の摩擦力トルクと、複数個の第２の摩擦力トルクから選択された一つの第２の摩擦力トルクと、算出された慣性力トルクと、算出された重力トルクとの和を算出し、この和と前記トルク指令値算出部に算出された前記トルク指令値との差の演算を、前記複数個の第２の摩擦力トルクのそれぞれに対して行い、前記差の絶対値が最小となる摩擦パラメータを選択し、この選択結果に基づいて、前記摩擦パラメータが更新されることを特徴とする。
【００１１】
一実施形態によるロボット制御装置は、図１に示すように、中央演算処理装置（ＣＰＵ）１と、アクチュエータ１００と、駆動部（アンプ）１０１と、駆動軸角度検出部１０２と、位置データ記憶部２００と、リンクパラメータ記憶部２０１と、摩擦パラメータ記憶部２０２と、ヤコビ行列記憶部２０３と、力／モーメントデータ記憶部２０４と、コンプライアンスモデル記憶部２０５と、を備えている。
【００１２】
駆動軸角度検出部１０２は、エンコーダ等の位置センサが使用可能であり、所定の周波数成分を除去するフィルタを備えていても良い。駆動軸角度検出部１０２は、駆動軸の位置の変位量（駆動軸の角度）を検出する。
【００１３】
図１に示したＣＰＵ１は、関節軸角度算出部１０３と、先端位置算出部１０４と、ヤコビ行列算出部１０５と、駆動トルク推定部１０６と、外部トルク算出部１０７と、外力算出部１０８と、力指令値生成部１０９と、力誤差算出部１１０と、コンプライアンス修正量算出部１１１と、位置指令値生成部１１２と、位置誤差算出部１１３と、関節角差分算出部１１４と、トルク指令値算出部１１５と、をハードウェア資源であるモジュール（論理回路）として論理的に備えている。
【００１４】
関節軸角度算出部１０３は、駆動軸角度検出部１０２により算出された駆動軸角度に基づいて、減速機の減速比と、伝達機構のもつ駆動軸と関節軸の比とに応じた関節軸角度を算出する。なお、関節軸に取り付けられる関節軸角度検出部（図示せず）により関節軸角度を直接求めてもよい。
【００１５】
先端位置算出部１０４は、リンクパラメータ記憶部２０１からリンクパラメータを読み出し、関節軸角度算出部１０３により算出された関節軸角度と、読み出されたリンクパラメータとを用いて、順運動学計算によりロボットの作業座標系における先端部の位置、すなわち先端位置を算出する。
【００１６】
ヤコビ行列算出部１０５は、関節軸角度算出部１０３により算出された関節軸角度から、ヤコビ行列を算出し、算出されたヤコビ行列をヤコビ行列記憶部２０３に格納する。ヤコビ行列とは、ロボットの作業座標系と関節座標系との間の微小変位関係を表現した行列である。ヤコビ行列をＪとすると、ロボットの先端位置の誤差Δｘと、関節角差分Δθは式（１）の関係を満たしている。
Δｘ＝ＪΔθ … （１）
【００１７】
駆動トルク推定部１０６は、駆動軸角度検出部１０２により検出された駆動軸角度と、関節軸角度算出部１０３により算出された関節軸角度とを用いて、ロボットの関節軸を駆動するのに必要な駆動トルクを推定する。この駆動トルク推定部１０６の一具体例を図２に示す。この具体例の駆動トルク推定部１０６は、図２に示すように、駆動軸速度算出部３００と、第１の摩擦力トルク算出部３０１と、慣性力トルク算出部３０２と、重力トルク算出部３０３と、加算部３０４と、第２の摩擦力トルク算出部３０５と、を備えている。
【００１８】
図２に示すように、駆動軸速度算出部３００は、駆動軸角度算出部１０２により算出された駆動軸角度に基づいて、例えば駆動軸角度の所定時間毎の差分を取ることにより駆動軸速度を算出する。第１の摩擦力トルク算出部３０１は、摩擦パラメータ記憶部２０２に格納された摩擦パラメータを読み出して、この読み出された摩擦パラメータと、駆動軸速度算出部３００により算出された駆動軸速度と、を用いてクーロン摩擦、粘性摩擦等に相当する第１の摩擦力トルクを算出する。
【００１９】
第２の摩擦力トルク算出部３０５について説明する。摩擦パラメータ記憶部２０２に格納された摩擦パラメータと、微小変化パラメータ記憶部３０６に格納された微小変化分とから、複数の摩擦パラメータの集合を用意する。例えば摩擦力τｆが駆動軸速度ω（＝ｄθ／ｄｔ）に対して、次の式（２）で表される関数
τf＝Ａ×ａｒｃｔａｎ（Ｂ×ω）＋ Ｃ …（２）
で定義されているとすると、摩擦パラメータは３種類のパラメータＡ、Ｂ、Ｃの組（Ａ、Ｂ、Ｃ）である。式（２）に示す関数をグラフで表すと図３に示すようになる。それぞれのパラメータに対して微小変化分ΔＡ、ΔＢ、ΔＣからＡ＋ΔＡ、Ａ―ΔＡ、Ｂ＋ΔＢ、Ｂ―ΔＢ、Ｃ＋ΔＣ、Ｃ―ΔＣを算出し、それらを組み合わせた摩擦パラメータの集合に対して第２の摩擦力トルクを算出する。この場合、摩擦パラメータの集合は、図４からわかるように、微小変化しないパラメータを含めると、２７個となる。この２７個の摩擦パラメータの集合のうち、パラメータＡ、Ｂ、Ｃが全く変化しない摩擦パラメータ（Ａ、Ｂ、Ｃ）に対する摩擦力は第１の摩擦力トルク算出部３０１により算出されたものである。そして、残りの２６通りの摩擦パラメータに対して、例えば式（２）を用いて、第２の摩擦力トルク算出部３０５によって第２の摩擦力トルクを算出する。したがって、第２の摩擦力トルクは、２６個の値を有する。
【００２０】
慣性力トルク算出部３０２は、関節軸角度算出部１０３により算出された関節軸角度を用いて関節角加速度を算出し、リンクパラメータ記憶部２０１からリンクパラメータを読み出し、この読み出されたリンクパラメータと、関節軸角度算出部１０３により算出された関節軸角度とを用いて各リンクの慣性モーメントを算出し、算出された関節角加速度と、算出された慣性モーメントとを用いて慣性力トルクを算出する。
【００２１】
重力トルク算出部３０３は、リンクパラメータ記憶部２０１からリンクパラメータを読み出し、この読み出されたリンクパラメータと、関節軸角度算出部１０３により算出された関節軸角度とを用いて、各リンクに作用する重力を算出し、この算出された重力を補償する重力トルクを算出する。
【００２２】
加算部３０４は、第１の摩擦力トルク算出部３０１により算出された第１の摩擦力トルクと、慣性力トルク算出ユニット３０２により算出された慣性力トルクと、重力トルク算出部３０３により算出された重力トルクとを加算し、それらの和を、推定した駆動トルクとして出力する。
【００２３】
このようにして駆動トルク推定部１０６により推定された駆動トルクと、トルク指令値算出部１１５により算出されたトルク指令値との差が、外部トルクとして図１に示した外部トルク算出部１０７によって算出される。
【００２４】
摩擦力トルク評価部３０７は、算出された第１の摩擦力トルクと、２６個の第２の摩擦力トルクから選択された一つの第２の摩擦力トルクと、算出された慣性力トルクと、算出された重力トルクとを加算し、これらの和とトルク指令値との差を演算する。この差の演算を、２６個の第２の摩擦力トルクに対して行う。そして、その差の絶対値が最小となる摩擦パラメータを摩擦パラメータの集合から選択して投票する。
【００２５】
パラメータ更新部３０８は、摩擦力トルク評価部３０７によって投票結果を摩擦パラメータ推定期間において集計し、予め定められた更新周期において最多票を集めた摩擦パラメータを摩擦パラメータ記憶部２０２へ格納することで、摩擦パラメータを更新させる。
【００２６】
図１に示す外力算出部１０８は、外部トルク算出部１０７により算出された外部トルクと、ヤコビ行列算出部１０５により算出されたヤコビ行列と、を用いて外力を算出する。仮想仕事の原理より、次の式（３）に示すように外部トルクτｄにヤコビ行列Ｊの転置行列ＪＴの逆行列を乗じることにより外力ｆｄが算出される。
ｆｄ＝（ＪＴ）−１τｄ …（３）
【００２７】
図１に示す力指令値生成部１０９は、力／モーメントデータ記憶部２０４に記憶された力／モーメントデータを読み出して、各制御周期における力／モーメント指令値を出力する。
【００２８】
力誤差算出部１１０は、力指令値生成部１０９で算出された力／モーメント指令値と、外力算出部１０８で算出された外力とに基づいて力誤差Δｆを算出する。
【００２９】
コンプライアンス修正量算出部１１１は、コンプライアンスモデル記憶部２０５からコンプライアンスモデルを読み出して、読み出されたコンプライアンスモデルを用いて、力誤差算出部１１０の出力Δｆに応じた位置修正量を算出する。ここで、コンプライアンスモデルとは、例えば式（４）に示すように接触対象との間において仮想的に慣性、粘性、剛性を想定したものである。
Ｍｄ２Δｘ／ｄｔ２＋ＤｄΔｘ／ｄｔ＋ＫΔｘ＝ＫｆΔｆ …（４）
ここで、Δｘは先端部に設定された作業座標系での誤差、ｄΔｘ／ｄｔは作業座標系での速度、ｄ２Δｘ／ｄｔ２は作業座標系での加速度ベクトル、Ｍは慣性行列、Ｄは粘性係数行列、Ｋは剛性係数行列、Ｋｆは力フィードバックゲイン行列である。力と位置の作用させる軸を切り替えるコンプライアンス選択行列は力フィードバックゲイン行列Ｋｆに含まれた形になっている。誤差の速度ｄΔｘ／ｄｔ及び加速度ベクトルｄ２Δｘ／ｄｔ２は、それぞれ位置誤差ベクトルΔｘの時間に関する一回差分、二回差分で近似できることから、コンプライアンス修正量Δｘｃｏｍｐは、次の式（５）を用いて算出することができる。
Δｘｃｏｍｐ＝１／Ｋ（ＫｆΔｆ−Ｍｄ２Δｘ／ｄｔ２−ＤｄΔｘ／ｄｔ） …（５）
【００３０】
位置指令値生成部１１２は、位置データ記憶部２００に記憶された目標先端位置データを読み出して、この目標先端位置データから各制御周期における補間された先端位置指令値を算出する。
【００３１】
位置誤差算出部１１３は、位置指令値生成部１１２により生成した先端位置指令値ｘＲと、先端位置算出部１０４により算出された現在の先端位置ｘと、コンプライアンス修正量算出部１１１により算出されたコンプライアンス修正量Δｘｃｏｍｐに基づいて、次の式（６）を用いて位置誤差Δｘを算出する。
Δｘ＝ｘＲ−ｘ＋Δｘｃｏｍｐ …（６）
【００３２】
関節角差分算出部１１４は、位置誤差算出部１１３から算出された位置誤差Δｘと、ヤコビ行列Ｊの逆行列Ｊ−１を用いて関節角差分Δθを、次の式（７）を用いて算出する。
Δθ＝Ｊ−１Δｘ …（７）
【００３３】
トルク指令値算出部１１５は、関節角差分算出部１１４により算出された関節角差分を積分することによりトルク指令値（制御目標値）を生成する。
【００３４】
駆動部１０１は、トルク指令値算出部１１５により算出されたトルク指令値にしたがってアクチュエータ１００を制御周期毎に駆動する。
【００３５】
位置データ記憶部２００、リンクパラメータ記憶部２０１、摩擦パラメータ記憶部２０２、ヤコビ行列記憶部２０３、力／モーメントデータ記憶部２０４、及びコンプライアンスモデル記憶部２０５としては、例えば半導体メモリ、磁気ディスク、光ディスク、光磁気ディスクや磁気テープ等が採用可能である。
【００３６】
位置データ記憶部２００は、位置指令値生成部１１１が先端位置指令値を生成するために使用する目標先端位置データ列を記憶している。リンクパラメータ記憶部２０１は、ロボットのリンクに関するリンクパラメータを記憶している。摩擦パラメータ記憶部２０２は、第１の摩擦力トルク算出部３０１が第１の摩擦力トルクを算出するために使用する、予め定速運転において速度−トルク関係から求めておいた摩擦パラメータのデータを記憶している。ヤコビ行列記憶部２０３は、ヤコビ行列算出部１０５により算出されるヤコビ行列を記憶している。力／モーメントデータ記憶部２０４は、力指令値生成部１０９が先端位置力指令値を生成するために使用する目標先端力データ列を記憶している。コンプライアンスモデル記憶部２０５は、予め設定されたコンプライアンスモデルを記憶している。
【００３７】
（ロボット制御方法）
次に、本実施形態によるロボット制御装置の動作を、図５に示すフローチャートを参照しながら説明する。
【００３８】
（ａ）まず、制御演算を開始し、位置指令値生成部１１２によって、位置データ記憶部２００から目標先端位置データ列が読み出されて、目標先端位置データ列に基づいて各制御周期における先端位置指令値が生成される（ステップ、Ｓ１００、Ｓ１０１）。
【００３９】
次に、ステップＳ１０２において、位置指令値生成部１１２により生成された先端位置指令値と、先端位置算出部１０４により算出された先端位置から、後述するコンプライアンス修正量を考慮した位置誤差が位置誤差算出部１１３によって算出される。続いて、ステップＳ１０３において、位置誤差算出部１１３により算出された位置誤差に対して、ヤコビ行列記憶部２０３から読み出したヤコビ行列を用いて式（７）に示すように、逆運動学計算を行って関節角差分が算出される。
【００４０】
その後、ステップＳ１０４において、関節角差分算出部１１４により算出された関節角差分を積分することによりトルク指令値がトルク指令値算出部１１５によって算出される。
【００４１】
次に、ステップＳ１０５において、トルク指令値算出部１１５により算出されたトルク指令値を制御目標値として駆動部１０１によってアクチュエータ１００が駆動されることにより、駆動軸３０６が駆動され、先端位置が制御される。
【００４２】
続いて、ステップＳ１０６において、制御演算の終了を確認し、ステップＳ１０７でサーボ処理を終了する。ステップＳ１０６において制御演算が終了でなければ後述するステップＳ１０８に進む。
【００４３】
（ｂ）次に、ステップＳ１０８において、駆動軸角度検出部１０２によって駆動軸角度を検出する。
【００４４】
続いて、ステップＳ１０９において、駆動軸角度検出部１０２により算出された駆動軸角度から、減速機の減速比などに基づいて関節軸角度算出部１０３によって関節軸角度が算出される。
【００４５】
ステップＳ１１０において、先端位置算出部１０４によって、リンクパラメータ記憶部２０１からリンクパラメータが読み出され、この読み出されたリンクパラメータと、関節軸角度算出部１０３により算出された関節軸角度とを用いて、順運動学計算により、先端位置が算出される。
【００４６】
ステップＳ１１１において、関節軸角度算出部１０３により算出された関節軸角度を用いて、ヤコビ行列算出部１０５によってヤコビ行列が算出される。
【００４７】
（ｃ）次に、ステップＳ１１２において、駆動軸角度検出部１０２により算出された駆動軸角度と、関節軸角度算出部１０３により算出された関節軸角度とを用いて駆動トルク推定部１０６によって駆動トルクが推定される。
【００４８】
ステップＳ１１３において、駆動トルク推定部１０６により推定された駆動トルクと、トルク指令値算出部１１５により算出された実際のトルク指令値との差から外部トルク算出部１０７によって外部トルクが算出される。
【００４９】
ステップＳ１１４において、外部トルク算出部１０７により算出された外部トルクと、ヤコビ行列算出部１０５により算出されたヤコビ行列とを用いて、外力算出部１０８によって式（３）に示す外力が算出される。
【００５０】
（ｄ）次に、ステップＳ１１５において、力指令値生成部１０９により力／モーメントデータ記憶部２０４から許容力／モーメントデータ列が読み出されて、この読み出された力／モーメントデータから力／モーメント指令値が算出される。
【００５１】
ステップＳ１１６において、力指令値生成部１０９により算出された力／モーメント指令値と、外力算出部１０８で算出された外力とを用いて力誤差算出部１１０によって力誤差が算出される。
【００５２】
（ｅ）次に、ステップＳ１１７において、コンプライアンス修正量算出部１１１によってコンプライアンスモデル記憶部２０５からコンプライアンスモデルが読み出され、コンプライアンスモデルを用いて式（５）に示すコンプライアンス修正量Δｘｃｏｍｐが算出され、このコンプライアンス修正量Δｘｃｏｍｐを用いて、力誤差算出部１１０により算出された力誤差に応じた先端位置修正量がコンプライアンス修正量算出部１１１によって算出される。
【００５３】
ステップＳ１０２に戻り、式（６）に示すように、位置指令値生成部１１２により生成された先端位置指令値と、先端位置算出部１０４により算出された先端位置との誤差に対して、コンプライアンス修正量算出部１１１から算出された修正量を考慮した位置誤差が位置誤差算出部１１３によって算出される。
【００５４】
駆動トルク推定処理としては、第１および第２の摩擦力トルクと、慣性力トルクと、重力トルクと用いて駆動トルクを推定する一例を示したが、これに限定されるものではない。例えば、遠心力やコリオリの力等のパラメータを更に考慮しても良い。
【００５５】
次に、本実施形態による摩擦パラメータ更新処理の動作を、図６に示すフローチャートを参照しながら説明する。なお本処理は先のロボット制御装置の制御周期と同じかもしくは数倍程度の頻度で行うことを想定するものである。
【００５６】
（ａ）摩擦パラメータ推定処理の開始するタイミング（ステップＳ２００）はステップＳ１１２における駆動トルク算出時である。ステップＳ２０１において、駆動軸角度検出部１０２により算出された駆動軸角度と、摩擦パラメータとから第１の摩擦力トルクが第１の摩擦力トルク算出部３０１によって算出される。ステップＳ２０２において、同じく駆動軸角度と、微小変化させたパラメータによる第２の摩擦力トルクが第２の摩擦力トルク算出部３０５によって算出される。
【００５７】
（ｂ）ステップＳ２０３において、外部トルク算出部１０７によって外部トルクを算出しこれを誤差とする。またステップＳ２０４において、摩擦力トルク評価部３０７によりトルク指令値および駆動トルクから誤差を算出する。
【００５８】
（ｃ）ステップＳ２０５において、それぞれの第１および第２の摩擦力トルクにおける誤差を比較し、誤差の絶対値が最小となる摩擦パラメータの集合に対して投票を行う。
【００５９】
（ｄ）ステップＳ２０６において、摩擦パラメータの更新周期であるかどうかを判定し、更新周期であればステップＳ２０７へ進んで、投票結果から最多票を得た摩擦パラメータを摩擦パラメータ記憶部２０２へ格納することで、摩擦パラメータを更新させる。摩擦パラメータの更新周期でなければステップＳ２０８へ進み、処理を終了する。
【００６０】
以上説明したように、本実施形態によれば、摩擦力トルクを高精度に推定し、力覚センサを使用しない場合においても、より高精度にロボットに作用する外力を推定することが可能となり、安定した力制御を実現することができる。
【００６１】
（第１変形例）
本実施形態の第１変形例として、摩擦力トルクの誤差の評価方法を説明する。上記実施形態では、誤差の絶対値が最小となる摩擦パラメータに投票したが、閾値を設けてトルク誤差の絶対値の最小値が上記閾値を超えない場合には、現在の摩擦パラメータに投票し、上記閾値以上となった場合には、誤差の絶対値が最小となる摩擦パラメータに投票するようにすると、摩擦パラメータの更新頻度が低くなるため、急激な変化が起こらず安定する。
【００６２】
また投票の結果、ほぼ同数の票を得た摩擦パラメータが複数存在した場合、最多得票の摩擦パラメータへと変更するのではなく、得票数に応じて微小変化分を線形補間させてもよい。例えば、
（Ａ＋ΔＡ、Ｂ―ΔＢ、Ｃ）の組がＮ1票
（Ａ＋ΔＡ、Ｂ、Ｃ＋ΔＣ）の組がＮ2票
であった場合
（Ａ＋ΔＡ、Ｂ―ΔＢ×Ｎ1／（Ｎ1＋Ｎ2）、Ｃ＋ΔＣ×Ｎ2／（Ｎ1＋Ｎ2））
とするのである。全てのケースにおいてこのような線形補間処理を行うと計算量が増加するため、得票数が同程度であった場合のみ実施する。
【００６３】
得票数が同じようになってしまう原因の一つは微小変化パラメータがやや大きいことが考えられる。したがって線形補間処理が続く場合、微小変化パラメータ記憶部３０６に格納された微小変化パラメータを変更する。
【００６４】
また、トルク指令値から慣性力トルク、重力トルク、そして摩擦パラメータにより算出した摩擦力トルクを引いた誤差には、摩擦パラメータの推定誤差に起因する誤差の他にも、ロボットに作用する外力に起因する外部トルクや、その他のパラメータの推定誤差に起因する誤差、あるいは上記実施形態において考慮していない力学的要因に起因したモデル化誤差などが含まれている。こうした外部要因を除き、摩擦パラメータの推定誤差のみに注目できるようにするために、実際にロボットを動作させて、摩擦パラメータを推定するための摩擦パラメータ推定試験を実施することも可能である。この場合は、ロボットに外力が作用しておらず、また決まった動作においてはモデル化誤差も一定となるため、誤差はパラメータ推定誤差に最も依存する。
【００６５】
一方、通常運転時においても誤差を評価することは可能であるが、上記外部要因が入り込む可能性を否定できない。そのため、誤差の評価の基準を下げてより安定した推定が行えるように配慮する。例えば第１変形例で述べたような、誤差の絶対値に対して設けた閾値を上げるとパラメータの更新頻度は低くなり、安定する。また更新周期を大きくすれば、より多くのデータが集まってからパラメータを更新するので信頼度が上がる。
【００６６】
（第２変形例）
本実施形態の第２変形例として、パラメータ変更履歴を利用したパラメータ変更について説明する。図７に示すように、摩擦パラメータ記憶部２０２内部に摩擦パラメータ変更履歴記憶部３０９と、パラメータ変更履歴データベース部３１０とを設ける。パラメータ更新部３０８によって更新された摩擦パラメータは摩擦パラメータ変更履歴記憶部３０９に格納され、摩擦力トルクを算出する際に摩擦パラメータとして利用される。摩擦パラメータの変更履歴をデータベース化したものがパラメータ変更履歴データベース部３１０へ格納される。パラメータ変更履歴データベース部３１０には同機種のロボットで計測した典型的な摩擦パラメータの変更履歴が格納されていて、微小変化パラメータ変更部３１１が両者を比較し、この比較結果に基づいて摩擦パラメータの微小変化分を修正し、微小変化パラメータ記憶部３０６へ格納する。
【００６７】
また、変更履歴には温度測定部３１２により計測された動作時の温度や、時間測定部３１３により計測された動作時刻も合わせて付加してもよい。通常、高温時や稼働時間経過とともに摩擦力は小さくなる傾向にある。温度や時刻をも合わせて記録しておくことで、もしパラメータの変更履歴がこうした一般的傾向から反している場合、故障などの異常診断にも利用できる。
【００６８】
（その他の実施形態）
上記のように、実施形態を説明したが、この開示の一部をなす論述及び図面はこの発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施の形態、実施例及び運用技術が明らかとなろう。
【００６９】
例えば、上記実施形態において、慣性力トルクの算出の際に、関節軸角度の時間に関する二回差分で算出された関節角加速度を用いているが、制御周期によっては時間遅れが大きくなりトルク誤差が大きくなりやすくなる。その場合は、リンクに搭載した加速度センサからリンクの加速度を検出し、検出した加速度を関節角加速度へ変換して、この関節角加速度を基に慣性力トルクを算出してもよい。また摩擦力のモデルとして複数の指数関数の線形和などを採用してもよく、いくつかのパラメータで特徴付けられるようなモデルに対しても本発明は適用できる。
【００７０】
また、ＣＰＵ１、位置データ記憶部２００、リンクパラメータ記憶部２０１、摩擦パラメータ記憶部２０２、ヤコビ行列記憶部２０３、力／モーメントデータ記憶部２０４、およびコンプライアンスモデル記憶部２０５等が制御対象であるロボットの内部に埋め込まれて一体化されていても良い。また、ＣＰＵ１、位置データ記憶部２００、リンクパラメータ記憶部２０１、摩擦パラメータ記憶部２０２、ヤコビ行列記憶部２０３、力／モーメントデータ記憶部２０４、およびコンプライアンスモデル記憶部２０５等が制御対象であるロボットの外部にあり、ロボットを有線或いは無線で遠隔制御することも可能である。
【００７１】
また、上記実施形態は、この発明の技術的思想を具体化するための装置や方法を例示するものであって、発明の範囲はそれらに限定されない。
【符号の説明】
【００７２】
１ 中央演算処理装置（ＣＰＵ）
１００ アクチュエータ
１０１ 駆動部
１０２ 駆動軸角度検出部
１０３ 関節軸角度算出部
１０４ 先端位置算出部
１０５ ヤコビ行列算出部
１０６ 駆動トルク推定部
１０７ 外部トルク算出部
１０８ 外力算出部
１０９ 力指令値生成部
１１０ 力誤差算出部
１１１ コンプライアンス修正量算出部
１１２ 位置指令値生成部
１１３ 位置誤差算出部
１１４ 関節角差分算出部
１１５ トルク指令値算出部
２００ 位置データ記憶部
２０１ リンクパラメータ記憶部
２０２ 摩擦パラメータ記憶部
２０３ ヤコビ行列記憶部
２０４ 力／モーメントデータ記憶部
２０５ コンプライアンスモデル記憶部
３００ 駆動軸速度算出部
３０１ 第１の摩擦力トルク算出部
３０２ 慣性力トルク算出部
３０３ 重力トルク算出部
３０４ 加算部
３０５ 第２の摩擦力トルク算出部
３０６ 微小変化パラメータ記憶部
３０７ 摩擦力トルク評価部
３０８ パラメータ更新部
３０９ パラメータ変更履歴記憶部
３１０ パラメータ変更履歴データベース部
３１１ 微小変化パラメータ変更部
３１２ 温度測定部
３１３ 時間測定部
【特許請求の範囲】
【請求項１】
関節軸および前記関節軸に駆動力を伝達する駆動軸を有するロボットを制御するロボット制御装置であって、
前記関節軸を制御周期毎に駆動するアクチュエータと、
前記駆動軸の角度を検出する駆動軸角度検出部と、
前記駆動軸の角度から前記関節軸の角度を算出する関節軸角度算出部と、
前記関節軸の角度から前記ロボットの先端位置を算出する先端位置算出部と、
前記先端位置の位置指令値と、前記先端位置との位置誤差を算出する位置誤差算出部と、
前記関節軸の角度に基づいて前記先端位置での作業座標系と関節座標系との間のヤコビ行列を算出するヤコビ行列算出部と、
関節角差分を算出する関節角差分算出部と、
前記関節角差分に基づいて関節のトルク指令値を算出するトルク指令値算出部と、
前記トルク指令値に基づいて前記アクチュエータを駆動する駆動部と、
前記関節軸の角度から前記アクチュエータを駆動するための駆動トルクを推定する駆動トルク推定部と、
推定された前記駆動トルクと前記トルク指令値との差を外部トルクとして算出する外部トルク算出部と、
前記外部トルクと前記ヤコビ行列から、前記先端位置に作用する外力を算出する外力算出部と、
前記先端位置におけるコンプライアンスモデルを記憶するコンプライアンスモデル記憶部と、
前記コンプライアンスモデルを用いて、前記外力に応じて前記先端位置での作業座標系位置に対するコンプライアンス修正量を算出し、前記コンプライアンス修正量を用いて前記位置誤差を修正するコンプライアンス修正量算出部と、
摩擦力トルクを近似するモデルの、少なくとも１つのパラメータを有する摩擦パラメータを記憶する摩擦パラメータ記憶部と、
摩擦力トルク評価部と、
を備え、
前記駆動トルク推定部は、前記駆動軸の角度から前記駆動軸の速度を算出する駆動軸速度算出部と、前記関節軸に作用する慣性力トルクを算出する慣性力トルク算出部と、前記関節軸に作用する重力を算出する重力トルク算出部と、算出された前記駆動軸の速度と、前記摩擦パラメータ記憶部に記憶された前記摩擦パラメータとを用いて第１の摩擦力トルクを推定する第１の摩擦力トルク算出部と、前記摩擦パラメータを変化させて複数個の第２の摩擦力トルクを算出する第２の摩擦力トルク算出部と、を有し、
前記摩擦力トルク評価部は、算出された第１の摩擦力トルクと、複数個の第２の摩擦力トルクから選択された一つの第２の摩擦力トルクと、算出された慣性力トルクと、算出された重力トルクとの和を算出し、この和と前記トルク指令値算出部に算出された前記トルク指令値とのトルク誤差の演算を、前記複数個の第２の摩擦力トルクのそれぞれに対して行い、前記トルク誤差に基づいて摩擦パラメータを選択し、
この選択結果に基づいて、前記摩擦パラメータが更新されることを特徴とするロボット制御装置。
【請求項２】
前記摩擦力トルク評価部は、前記トルク誤差の絶対値が最小となる摩擦パラメータを選択することを特徴とする請求項１記載のロボット制御装置。
【請求項３】
前記摩擦力トルク評価部は、前記トルク誤差の絶対値の最小値が所定の閾値を超えない場合には、現在の摩擦パラメータを選択し、前記閾値以上となった場合には、前記トルク誤差の絶対値が最小となる摩擦パラメータを選択することを特徴とする請求項１記載のロボット制御装置。
【請求項４】
前記摩擦力トルク評価部によって選択された前記摩擦パラメータを、摩擦パラメータ推定期間において集計し、予め定められた更新周期において最も多く選択された摩擦パラメータを前記摩擦パラメータ記憶部へ格納し、前記摩擦パラメータを更新するパラメータ更新部部を更に備えていることを特徴とする請求項１乃至３のいずれかに記載のロボット制御装置。
【請求項５】
前記パラメータ記憶部は、
変更された前記摩擦パラメータを記憶するとともに前記摩擦パラメータの変更の履歴を記憶するパラメータ変更履歴記憶部と、
前記摩擦パラメータの変更履歴をデータベース化した第１変更履歴として記憶するとともに、同機種のロボットで計測した摩擦パラメータの変更履歴をデータベース化した第２変更履歴として記憶するパラメータ変更履歴データベース部と、
を備え、前記ロボット制御装置は、
前記第１変更履歴と前記第２変更履歴と比較し、この比較結果に基づいて前記摩擦パラメータの微小変化分を変更する微小変化パラメータ変更部を更に
を備えていることを特徴とする請求項１乃至４のいずれかに記載のロボット制御装置。
【請求項６】
ロボットの動作時の温度を測定する温度測定部と、
前記ロボットの通算動作時間を測定する時間測定部と、
を更に備え、
前記第１および第２変更履歴に前記温度データや前記通算動作時間を付加することを特徴とする請求項５記載のロボット制御装置。
【請求項１】
関節軸および前記関節軸に駆動力を伝達する駆動軸を有するロボットを制御するロボット制御装置であって、
前記関節軸を制御周期毎に駆動するアクチュエータと、
前記駆動軸の角度を検出する駆動軸角度検出部と、
前記駆動軸の角度から前記関節軸の角度を算出する関節軸角度算出部と、
前記関節軸の角度から前記ロボットの先端位置を算出する先端位置算出部と、
前記先端位置の位置指令値と、前記先端位置との位置誤差を算出する位置誤差算出部と、
前記関節軸の角度に基づいて前記先端位置での作業座標系と関節座標系との間のヤコビ行列を算出するヤコビ行列算出部と、
関節角差分を算出する関節角差分算出部と、
前記関節角差分に基づいて関節のトルク指令値を算出するトルク指令値算出部と、
前記トルク指令値に基づいて前記アクチュエータを駆動する駆動部と、
前記関節軸の角度から前記アクチュエータを駆動するための駆動トルクを推定する駆動トルク推定部と、
推定された前記駆動トルクと前記トルク指令値との差を外部トルクとして算出する外部トルク算出部と、
前記外部トルクと前記ヤコビ行列から、前記先端位置に作用する外力を算出する外力算出部と、
前記先端位置におけるコンプライアンスモデルを記憶するコンプライアンスモデル記憶部と、
前記コンプライアンスモデルを用いて、前記外力に応じて前記先端位置での作業座標系位置に対するコンプライアンス修正量を算出し、前記コンプライアンス修正量を用いて前記位置誤差を修正するコンプライアンス修正量算出部と、
摩擦力トルクを近似するモデルの、少なくとも１つのパラメータを有する摩擦パラメータを記憶する摩擦パラメータ記憶部と、
摩擦力トルク評価部と、
を備え、
前記駆動トルク推定部は、前記駆動軸の角度から前記駆動軸の速度を算出する駆動軸速度算出部と、前記関節軸に作用する慣性力トルクを算出する慣性力トルク算出部と、前記関節軸に作用する重力を算出する重力トルク算出部と、算出された前記駆動軸の速度と、前記摩擦パラメータ記憶部に記憶された前記摩擦パラメータとを用いて第１の摩擦力トルクを推定する第１の摩擦力トルク算出部と、前記摩擦パラメータを変化させて複数個の第２の摩擦力トルクを算出する第２の摩擦力トルク算出部と、を有し、
前記摩擦力トルク評価部は、算出された第１の摩擦力トルクと、複数個の第２の摩擦力トルクから選択された一つの第２の摩擦力トルクと、算出された慣性力トルクと、算出された重力トルクとの和を算出し、この和と前記トルク指令値算出部に算出された前記トルク指令値とのトルク誤差の演算を、前記複数個の第２の摩擦力トルクのそれぞれに対して行い、前記トルク誤差に基づいて摩擦パラメータを選択し、
この選択結果に基づいて、前記摩擦パラメータが更新されることを特徴とするロボット制御装置。
【請求項２】
前記摩擦力トルク評価部は、前記トルク誤差の絶対値が最小となる摩擦パラメータを選択することを特徴とする請求項１記載のロボット制御装置。
【請求項３】
前記摩擦力トルク評価部は、前記トルク誤差の絶対値の最小値が所定の閾値を超えない場合には、現在の摩擦パラメータを選択し、前記閾値以上となった場合には、前記トルク誤差の絶対値が最小となる摩擦パラメータを選択することを特徴とする請求項１記載のロボット制御装置。
【請求項４】
前記摩擦力トルク評価部によって選択された前記摩擦パラメータを、摩擦パラメータ推定期間において集計し、予め定められた更新周期において最も多く選択された摩擦パラメータを前記摩擦パラメータ記憶部へ格納し、前記摩擦パラメータを更新するパラメータ更新部部を更に備えていることを特徴とする請求項１乃至３のいずれかに記載のロボット制御装置。
【請求項５】
前記パラメータ記憶部は、
変更された前記摩擦パラメータを記憶するとともに前記摩擦パラメータの変更の履歴を記憶するパラメータ変更履歴記憶部と、
前記摩擦パラメータの変更履歴をデータベース化した第１変更履歴として記憶するとともに、同機種のロボットで計測した摩擦パラメータの変更履歴をデータベース化した第２変更履歴として記憶するパラメータ変更履歴データベース部と、
を備え、前記ロボット制御装置は、
前記第１変更履歴と前記第２変更履歴と比較し、この比較結果に基づいて前記摩擦パラメータの微小変化分を変更する微小変化パラメータ変更部を更に
を備えていることを特徴とする請求項１乃至４のいずれかに記載のロボット制御装置。
【請求項６】
ロボットの動作時の温度を測定する温度測定部と、
前記ロボットの通算動作時間を測定する時間測定部と、
を更に備え、
前記第１および第２変更履歴に前記温度データや前記通算動作時間を付加することを特徴とする請求項５記載のロボット制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２０１２−２４８７７（Ｐ２０１２−２４８７７Ａ）
【公開日】平成２４年２月９日（２０１２．２．９）
【国際特許分類】
【出願番号】特願２０１０−１６５１８８（Ｐ２０１０−１６５１８８）
【出願日】平成２２年７月２２日（２０１０．７．２２）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
【公開日】平成２４年２月９日（２０１２．２．９）
【国際特許分類】
【出願日】平成２２年７月２２日（２０１０．７．２２）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
[ Back to top ]