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

Abstract

【課題】力覚センサを使用せずに、外力をより高精度に推定し、安定した力制御を実現できるロボット制御装置を提供する。
【解決手段】先端位置と位置指令値との誤差を算出する誤差算出部１１２と、誤差から関節角差分を算出する関節角差分算出部１１３と、関節角差分からトルク指令値を算出するトルク指令値算出部１１４と、アクチュエータ１１５の駆動トルクを推定する駆動トルク推定部１０７と、駆動トルク及びトルク指令値との差を外部トルクとして算出する外部トルク算出部１０８と、関節軸角度に基づいてヤコビ行列を算出するヤコビ行列算出部１０５と、ヤコビ行列及び外部トルクから外力を算出する外力算出部１０９と、コンプライアンスモデル及び外力から位置指令値に対する修正量を算出する修正量算出部１１０とを備える。
【選択図】図１

Description

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

ロボットのアーム先端部などの先端（以下、単に「先端」という。）が対象物と接触しながら、作業座標系での位置及び接触力を制御する、いわゆる力制御では、先端において作用している外力を推定し、外力に応じた動作の修正を柔軟に行うことが重要である。先端に力覚センサを取り付けた場合、外力を高精度に測定できるが、力覚センサは高価である上に衝撃に対して脆弱であるため、力覚センサの使用が敬遠されることが多い。

力覚センサを使用しない場合においては、外力を推定するために、ロボットに作用している外力の影響で発生するアクチュエータの外部トルクを推定する必要がある。外部トルクは、アクチュエータの運動に必要な駆動トルクを推定し、駆動部（アンプ）への実際のトルク指令値から推定した駆動トルクを引くことにより算出される。例えば、関節ごとに関節指令から駆動トルクを算出し、トルク指令値との差から外部トルクを推定し、これを作業座標系における外力へと変換する手法が知られている（例えば、特許文献１参照。）。

ところが、駆動トルクを推定するには、動力学モデルのパラメータを高精度に推定しておくことが必要であるが、実現には困難を伴う。特に摩擦力は駆動トルクの中で影響力の大きい項であるが、その摩擦係数は、アクチュエータの動作速度・動作方向・動作履歴・動作環境、また先端に取り付けられた負荷等により大きく変化するため、正確に推定することは困難である。作業座標系での運動では関節軸での動作方向が頻繁かつ複雑に変化するため、特許文献１のような各関節軸での外力推定だけでは充分とはいえない。このように、外力推定において力覚センサを使用しない場合において、外力をより高精度に推定し、安定した力制御を実現することは困難であった。
特許第４０５４９８４号

本発明の目的は、力覚センサを使用せずに、外力をより高精度に推定し、安定した力制御を実現できるロボット制御装置を提供することである。

本願発明の一態様によれば、関節軸及び関節軸に駆動力を伝達する駆動軸を有するロボットを制御するロボット制御装置であって、（イ）関節軸を駆動するアクチュエータと、（ロ）駆動軸の角度を検出する駆動軸角度検出部と、（ハ）駆動軸の角度から関節軸の角度を算出する関節軸角度算出部と、（ニ）関節軸の角度からロボットの先端位置を算出する先端位置算出部と、（ホ）先端位置の位置指令値を生成する位置指令値生成部と、（ヘ）先端位置と位置指令値との誤差を算出する誤差算出部と、（ト）誤差から逆運動学計算により関節角差分を算出する関節角差分算出部と、（チ）関節角差分を積分することによりトルク指令値を算出するトルク指令値算出部と、（リ）トルク指令値に基づいてアクチュエータを駆動させる駆動部と、（ヌ）関節軸の角度からアクチュエータを駆動させるための駆動トルクを推定する駆動トルク推定部と、（ル）推定された駆動トルク及びトルク指令値との差を外部トルクとして算出する外部トルク算出部と、（ヲ）関節軸の角度に基づいてロボットの作業座標系と関節座標系との間のヤコビ行列を算出するヤコビ行列算出部と、（ワ）ヤコビ行列及び外部トルクから、先端位置に作用する外力を算出する外力算出部と、（カ）先端位置におけるコンプライアンスモデルを記憶するコンプライアンスモデル記憶部と、（ヨ）コンプライアンスモデルを用いて、外力に応じて位置指令値に対する修正量を算出する修正量算出部とを備えるロボット制御装置が提供される。

本発明によれば、力覚センサを使用せずに、外力をより高精度に推定し、安定した力制御を実現できるロボット制御装置を提供することができる。

次に、図面を参照して、本発明の実施の形態を説明する。以下の図面の記載において、同一又は類似の部分には同一又は類似の符号を付している。ただし、図面は模式的なものであり、具体的な寸法は以下の説明を参酌して判断すべきものである。又、図面相互間においても互いの寸法の関係や比率が異なる部分が含まれていることはもちろんである。

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

本発明の実施の形態に係るロボット制御装置は、図１に示すように、中央演算処理装置（ＣＰＵ）１００、駆動部（アンプ）１０１、駆動軸角度検出部１０２、位置データ記憶部２００、摩擦係数記憶部２０１、リンクパラメータ記憶部２０２、閾値記憶部２０３、ヤコビ行列記憶部２０４及びコンプライアンスモデル記憶部２０５を備える。

本発明の実施の形態において制御対象の一例とするロボットは、図２に模式的に示すように、本体３００と、本体３００に設けられた可動部３１０を備える。可動部３１０は、複数のリンク３０１，３０２と、駆動プーリ３０３及び従動プーリ３０４等から構成される複数の関節３０６，３０７と、駆動プーリ３０３及び従動プーリ３０４に巻きつけた伝達機構３０８を有する。

図３に模式的に示すように、駆動プーリ３０３、減速機３０９、アクチュエータ１１５及び駆動軸角度検出部１０２が、駆動軸（関節）３０６に取り付けられている。アクチュエータ１１５が回転駆動し、減速機３０９が回転数を落としトルクを上げる。一方、従動プーリ３０４及び関節軸角度検出部３０５が関節軸（関節）３０７に取り付けられている。駆動軸３０６の回転により、駆動プーリ３０３、伝達機構（ベルト）３０８及び従動プーリ３０４を介して関節軸３０７が回転駆動する。本発明の実施の形態では、簡単のために、駆動軸３０６及び関節軸３０７に対して制御する場合について述べる。

駆動軸角度検出部１０２及び関節軸角度検出部３０５のそれぞれは、エンコーダ等の位置センサが使用可能であり、所定の周波数成分を除去するフィルタを備えていても良い。駆動軸角度検出部１０２は、駆動軸３０６の位置の変位量（駆動軸角度）を検出する。関節軸角度検出部３０５は、関節軸３０７の位置の変位量（関節軸角度）を検出する。

図１に示したＣＰＵ１００は、関節軸角度算出部１０３、先端位置算出部１０４、ヤコビ行列算出部１０５、駆動トルク推定部１０７、外部トルク算出部１０８、外力算出部１０９、修正量算出部１１０、位置指令値生成部１１１、誤差算出部１１２、関節角差分算出部１１３及びトルク指令値算出部１１４をハードウェア資源であるモジュール（論理回路）として論理的に備える。

関節軸角度算出部１０３は、駆動軸角度検出部１０２により算出された駆動軸角度から、減速機３０９の減速比など、伝達機構３０８のもつ駆動軸３０６と関節軸３０７の比に応じて関節軸角度を算出する。なお、関節軸３０７に取り付けた関節軸角度検出部３０５により関節軸角度を直接算出してもよい。

先端位置算出部１０４は、リンクパラメータ記憶部２０２からリンクパラメータを読み出し、関節軸角度算出部１０３により算出された関節軸角度から、リンクパラメータを用いて順運動学計算によりロボットの作業座標系における先端位置を算出する。

ヤコビ行列算出部１０５は、関節軸角度算出部１０３により算出された関節軸角度から、ヤコビ行列を算出し、ヤコビ行列記憶部２０４に格納する。ヤコビ行列とは、ロボットの作業座標系と関節座標系との間の微小変位関係を表現した行列である。ヤコビ行列をＪとして、誤差Δｘと関節角差分Δθには式（１）の関係が成立している。

駆動トルク推定部１０７は、関節軸角度算出部１０３により算出された関節軸角度から、ロボットの関節軸３０７を駆動するのに必要な駆動トルクを推定する。駆動トルク推定部１０７は、図４に示すように、駆動軸速度算出部４００、重力トルク算出部４０３、慣性力トルク算出部４０２、摩擦力トルク算出部４０１及び加算部４０４を備える。

駆動軸速度算出部４００は、関節軸角度算出部１０３により算出された関節軸角度から例えば時間差分を取ることにより駆動軸速度を算出する。摩擦力トルク算出部４０１は、摩擦係数記憶部２０１に格納された摩擦係数を読み出して、駆動軸速度算出部４００により算出された駆動軸速度と摩擦係数からクーロン摩擦、粘性摩擦等に相当する摩擦力トルクを算出する。

慣性力トルク算出部４０２は、速度算出ユニット４１０、加速度算出ユニット４１１、慣性モーメント算出ユニット４１２及び慣性力トルク算出ユニット４１３を備える。速度算出ユニット４１０は、関節軸角度算出部１０３により算出された関節軸角度から関節角速度を算出する。加速度算出ユニット４１１は、速度算出ユニット４１０により算出された関節角速度から関節角加速度を算出する。慣性モーメント算出ユニット４１２は、リンクパラメータ記憶部２０２からリンクパラメータを読み出し、リンクパラメータ及び関節軸角度算出部１０３により算出された関節軸角度から各リンク３０１，３０２の慣性モーメントを算出する。慣性力トルク算出ユニット４１３は、加速度算出ユニット４１１により算出された関節角加速度、及び慣性モーメント算出ユニット４１２により算出された慣性モーメントから慣性力トルクを算出する。

重力トルク算出部４０３は、リンクパラメータ記憶部２０２からリンクパラメータを読み出し、リンクパラメータを用いて、関節軸角度算出部１０３により算出された関節軸角度から各リンク３０１，３０２に作用する重力を算出し、この重力を補償する重力トルクを算出する。

加算部４０４は、摩擦力トルク算出部４０１により算出された摩擦力トルク、慣性力トルク算出ユニット４１３により算出された慣性力トルク及び重力トルク算出部４０３により算出された重力トルクを加算して推定した駆動トルクとして算出する。

図１に示した外部トルク算出部１０８は、駆動トルク推定部１０７により推定された駆動トルクと、トルク指令値算出部１１４により算出されたトルク指令値との差を外部トルクとして算出する。

外力算出部１０９は、外部トルク算出部１０８により算出された外部トルクからヤコビ行列算出部１０５により算出されたヤコビ行列を用いて外力を算出する。外力算出部１０９は、図５に示すように、不感帯処理ユニット５０１、飽和処理ユニット５０２、トルク−力変換ユニット５０３、座標変換ユニット５０４及び閾値設定ユニット５０５を備える。

不感帯処理ユニット５０１は、閾値記憶部２０３に記憶された最小閾値（最小外部トルク）τ_dminを読み出して、外部トルク算出部１０８により算出された各軸における外部トルクτ_dが最小閾値より小さいか判定する。式（２）のように外部トルクτ_dが最小閾値τ_dminより小さければ、ノイズの可能性が高い等の理由から、不感帯として外部トルクτ_dを０とみなす。

飽和処理ユニット５０２は、閾値記憶部２０３に記憶された最大閾値（最大外部トルク）τ_dmaxを読み出して、外部トルク算出部１０８により算出された各軸における外部トルクτ_dが最大閾値τ_dmaxより大きいか判定する。式（３）のように外部トルクτ_dが最大外部トルクを越えると判定された場合、先端にて接触している作業対象を破損することを避けるため、外部トルクτ_dを最大閾値τ_dmaxの値とする。

トルク−力変換ユニット５０３は、ヤコビ行列算出部１０５により算出されたヤコビ行列を用いて、外部トルクを作業座標系における外力へと変換する。仮想仕事の原理より、式（４）のように外部トルクτ_dにヤコビ行列Ｊの転置行列Ｊ^Tの逆行列を乗じることにより外力ｆ_dを算出する。

ここで、式（４）の外力ｆ_dは先端で作用していると想定したときの外力である。外力ｆdが先端以外を作用点としている場合は、座標変換ユニット５０４が、外力ｆdを実際の作用点での外力に座標変換する。

閾値設定ユニット５０５は、ヤコビ行列記憶部２０４に記憶されているヤコビ行列に基づいて、最小閾値τ_dmin及び最大閾値τ_dmaxをそれぞれ設定し、閾値記憶部２０３に格納する。ここで、摩擦力はアクチュエータ１１５の速度や動作方向によりその摩擦係数が変化する。特に静止時からの動き出し時においては最大静止摩擦力が支配的となり、速度が増加するに従って支配的となる粘性摩擦力や動摩擦力に比べて大きくなっている。速度０付近における不連続性が摩擦力トルク、さらには駆動トルクの推定誤差に大きな影響を及ぼす。そこで、軸速度が０近傍においては不感帯を大きくする、すなわち最小閾値τ_dminを大きくするように最小閾値τ_dminを設定する。軸速度が０近傍であるかどうかは、ヤコビ行列の行列式が減少したり、ヤコビ行列の対角成分の絶対値が減少するなどの現象に基づいて判断する。特に作業座標系における直線運動中に、ある関節の動作方向のみ変化することが起こりうるが、このとき変化の前後においてヤコビ行列は極小値を取る。よって、閾値設定ユニット５０５は、ヤコビ行列記憶部２０４に記憶されているヤコビ行列の変化率を算出し、このヤコビ行列の変化率に基づいて軸速度が０近傍であるかどうかを判断する。

図１に示した修正量算出部１１０は、コンプライアンスモデル記憶部２０５からコンプライアンスモデルを読み出して、コンプライアンスモデルを用いて、外力算出部１０９により算出された外力に応じた先端位置の修正量を算出する。ここで、コンプライアンスモデルとは、例えば式（５）のように先端と接触対象との間において仮想的に慣性、粘性、剛性を想定したものである。

ここで、Δｘは作業座標系での誤差、Δ

は作業座標系での速度、Δ

は作業座標系での加速度ベクトル、Ｍは慣性行列、Ｄは粘性係数行列、Ｋは剛性係数行列、Ｋ_fは力フィードバックゲイン行列である。力と位置の作用させる軸を切り替えるコンプライアンス選択行列は力フィードバックゲイン行列Ｋ_fに含んだ形になっている。誤差の速度Δ

及び加速度ベクトルΔ

は、それぞれ位置誤差ベクトルΔｘの時間一回差分、二回差分で近似できることから、コンプライアンスモデルにおける先端位置の修正量Δｘ_compは式（６）のように算出できる。

位置指令値生成部１１１は、位置データ記憶部２００に記憶された目標先端位置データを読み出して、目標先端位置データから各制御周期における補間された先端位置指令値を算出する。

誤差算出部１１２が、位置指令値生成部１１１により生成した先端位置指令値ｘ_R、先端位置算出部１０４により算出された現在の先端位置ｘ、及び修正量算出部１１０により算出された修正量Δｘ_compから、式（７）のように修正量Δｘ_compを考慮した誤差Δｘを算出する。

関節角差分算出部１１３は、誤差算出部１１２により算出された誤差Δｘからヤコビ行列Ｊの逆行列を用いて関節角差分Δθを式（８）のように算出する。

トルク指令値算出部１１４は、関節角差分算出部１１３により算出された関節角差分を積分することによりトルク指令値（制御目標値）を生成する。駆動部１０１は、トルク指令値算出部１１４により算出されたトルク指令値にしたがってアクチュエータ１１５を駆動する。

位置データ記憶部２００、摩擦係数記憶部２０１、リンクパラメータ記憶部２０２、閾値記憶部２０３、ヤコビ行列記憶部２０４及びコンプライアンスモデル記憶部２０５としては、例えば半導体メモリ、磁気ディスク、光ディスク、光磁気ディスクや磁気テープ等が採用可能である。

位置データ記憶部２００は、位置指令値生成部１１１が先端位置指令値を生成するために使用する目標先端位置データ列を記憶している。摩擦係数記憶部２０１は、摩擦力トルク算出部４０１が摩擦力トルクを算出するために使用する、予め定速運転において速度−トルク関係から求めておいた摩擦係数データを記憶している。リンクパラメータ記憶部２０２は、ロボットのリンク３０１，３０２に関するリンクパラメータを記憶している。閾値記憶部２０３は、予め作業内容及び動作速度によって設定された最小閾値τ_dmin及び最大閾値τ_dmaxや、閾値設定ユニット５０５により設定された最小閾値τ_dmin及び最大閾値τ_dmaxを記憶している。ヤコビ行列記憶部２０４は、ヤコビ行列算出部１０５により算出されるヤコビ行列を記憶している。コンプライアンスモデル記憶部２０５は、予め設定されたコンプライアンスモデルや、修正量算出部１１０により変更されたコンプライアンスモデルを記憶している。

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

（イ）ステップＳ１００において制御演算を開始する。ステップＳ１０１において、位置指令値生成部１１１が、位置データ記憶部２００から目標先端位置データ列を読み出して、目標先端位置データ列に基づいて各制御周期における先端位置指令値を生成する。ステップＳ１０２において、誤差算出部１１２が、位置指令値生成部１１１により生成された先端位置指令値と、先端位置算出部１０４により算出された先端位置との誤差に、後述する先端位置の修正量を考慮した誤差を算出する。ステップＳ１０３において、関節角差分算出部１１３が、誤差算出部１１２により算出された誤差に対して、ヤコビ行列記憶部から読み出したヤコビ行列を用いて式（８）にあるような逆運動学計算を行って関節角差分を算出する。ステップＳ１０４において、トルク指令値算出部１１４が、関節角差分算出部１１３により算出された関節角差分を積分することによりトルク指令値を算出する。ステップＳ１０５において、駆動部１０１が、トルク指令値算出部１１４により算出されたトルク指令値を制御目標値としてアクチュエータ１１５を駆動することにより、駆動軸３０６を駆動させ、先端位置を制御する。ステップＳ１０６において、制御演算の終了を確認し、ステップＳ１０７で処理を終了する。ステップS１０６において制御演算が終了でなければステップＳ１０８に進む。

（ロ）ステップＳ１０８において、駆動軸角度検出部１０２が、駆動軸角度を検出する。ステップＳ１０９において、関節軸角度算出部１０３が、駆動軸角度検出部１０２により算出された駆動軸角度から、減速機の減速比などに基づいて関節軸角度を算出する。ステップＳ１１０において、先端位置算出部１０４が、リンクパラメータ記憶部２０２からリンクパラメータを読み出し、リンクパラメータを用いて、関節軸角度算出部１０３により算出された関節軸角度から順運動学計算により先端位置を算出する。ステップＳ１１１において、ヤコビ行列算出部１０５が、関節軸角度算出部１０３により算出された関節軸角度からヤコビ行列を算出する。

（ハ）ステップＳ１１２において、駆動トルク推定部１０７が、駆動軸角度検出部１０２により算出された駆動軸角度、及び関節軸角度算出部１０３により算出された関節軸角度から駆動トルクを推定する。ステップＳ１１３において、外部トルク算出部１０８が、駆動トルク推定部１０７により推定された駆動トルクと、トルク指令値算出部１１４により算出された実際のトルク指令値との差から外部トルクを算出する。ステップＳ１１４において、外力算出部１０９は、外部トルク算出部１０８により算出された外部トルクからヤコビ行列算出部１０５により算出されたヤコビ行列を用いて式（４）のように外力を算出する。ステップＳ１１５において、修正量算出部１１０が、コンプライアンスモデル記憶部２０５からコンプライアンスモデルを読み出し、コンプライアンスモデルを用いて式（６）のように外力算出部１０９により算出された外力に応じた先端位置の修正量を算出する。ステップＳ１０２に戻り、誤差算出部１１２が、式（７）のように位置指令値生成部１１１により生成された先端位置指令値と、先端位置算出部１０４により算出された先端位置との誤差に、修正量算出部１１０により算出された修正量を考慮した誤差を算出する。

＜駆動トルク推定処理＞
次に、図６に示したステップＳ１１２の駆動トルク推定処理を図７のフローチャートを参照しながら説明する。

（イ）ステップＳ２００において、駆動トルク推定処理を開始する。ステップＳ２０１において、駆動軸速度算出部４００が、駆動軸角度検出部１０２により検出された駆動軸角度から時間差分を取ることにより駆動軸速度を算出する。更に、摩擦力トルク算出部４０１が、摩擦係数記憶部２０１から摩擦係数を読み出し、摩擦係数及び駆動軸速度算出部４００により算出された駆動軸速度から摩擦力トルクを算出する。

（ロ）ステップＳ２０２において、速度算出ユニット４１０が、関節軸角度算出部１０３により算出された関節軸角度の時間差分を取ることにより関節角速度を算出する。ステップＳ２０３において、加速度算出ユニット４１１が、速度算出ユニット４１０により関節角速度の時間差分を取ることにより関節角加速度を算出する。ステップＳ２０４において、慣性モーメント算出ユニット４１２が、リンクパラメータ記憶部２０２からリンクパラメータを読み出し、関節軸角度算出部１０３により算出された関節軸角度とリンクパラメータから各関節におけるリンクの慣性モーメントを算出する。ステップＳ２０５において、慣性力トルク算出ユニット４１３が、加速度算出ユニット４１１により算出された関節角加速度及び慣性モーメント算出ユニット４１２により算出された慣性モーメントから慣性力トルクを算出する。

（ハ）ステップＳ２０６において、重力トルク算出部４０３が、リンクパラメータ記憶部２０２からリンクパラメータを読み出し、関節軸角度算出部１０３により算出された関節軸角度関節軸角度とリンクパラメータから各リンク３０１，３０２に作用する重力を算出し、これを補償する重力トルクを算出する。

（ニ）ステップＳ２０７において、加算部４０４が、摩擦力トルク算出部４０１により算出された摩擦力トルク、慣性力トルク算出ユニット４１３により算出された慣性力トルク及び重力トルク算出部４０３により算出された重力トルクを加算して推定した駆動トルクとして算出する。ステップＳ２０８において駆動トルク推定処理を終了する。

＜外力算出処理＞
次に、図６に示したステップＳ１１４の外力算出処理を図８のフローチャートを参照しながら説明する。

（イ）ステップＳ３００において、外力算出処理を開始する。ステップＳ３０１において、不感帯処理ユニット５０１が、閾値記憶部２０３に記憶された最小閾値（最小外部トルク）を読み出して、外部トルク算出ユニットにより算出された各軸における外部トルクが最小閾値より小さいか判定する。外部トルクが最小閾値より小さければ、ステップＳ３０２に進み、不感帯処理ユニット５０１が、式（２）のように不感帯として外部トルクを０とみなす。外部トルクが最小閾値以上と判定された場合、外部トルクの値をそのまま維持する。

（ロ）ステップＳ３０３において、飽和処理ユニット５０２が、閾値記憶部２０３に記憶された最大閾値（最大外部トルク）を読み出して、外部トルクが最大閾値より大きいか判定する。外部トルクが最大閾値より大きいと判定された場合、ステップＳ３０４に進み、先端にて接触している作業対象を破損してしまうことを避けるため、式（３）のように外部トルクを最大閾値の値とする。

（ハ）ステップＳ３０５において、トルク−力変換ユニット５０３が、ヤコビ行列算出部１０５により算出されたヤコビ行列を用いて式（４）のように仮想仕事の原理より、外部トルクにヤコビ行列の転置行列の逆行列を乗じることにより外部トルクを外力に変換する。ステップＳ３０６において、外力が先端以外を作用点としている場合は、座標変換ユニット５０４が、先端以外を作用点としている外力を作業座標系における外力へと座標変換する。

（ニ）ステップＳ３０７において、閾値設定ユニット５０５が、ヤコビ行列記憶部２０４からヤコビ行列を読み出して、ヤコビ行列の変化率に応じて最小閾値及び最大閾値を設定する。ステップＳ３０８において外力算出処理を終了する。

以上説明したように、本発明の実施の形態によれば、ロボットの作業形態や動作速度などに基づいて、摩擦係数や不感帯閾値などのアルゴリズムに含まれるパラメータを動的に変更させて、ロボットに作用する外力により発生した外部トルクをより高精度に算出することができ、正確な先端位置の修正量を算出することができる。この結果、力覚センサを用いることなく、作業座標系における位置及び力を制御する、安定した力制御を実現できる。

なお、図６〜図８に示した一連の手順、例えば：駆動軸角度検出部１０２が、駆動軸角度を検出するステップ；関節軸角度算出部１０３が、駆動軸角度から関節軸角度を算出するステップ；先端位置算出部１０４が、関節軸の角度からロボットの先端位置を算出するステップ；位置指令値生成部１１１が、先端位置の位置指令値を生成するステップ；誤差算出部１１２が、先端位置と位置指令値との誤差を算出するステップ；関節角差分算出部１１３が、誤差から逆運動学計算により関節角差分を算出するステップ；トルク指令値算出部１１４が、関節角差分を積分することによりトルク指令値を算出するステップ；駆動部１０１が、トルク指令値に基づいてアクチュエータ１１５を駆動させるステップ；駆動トルク推定部１０７が、関節軸角度からアクチュエータ１１５を駆動させるための駆動トルクを推定するステップ；外部トルク算出部１０８が、推定された駆動トルク及びトルク指令値との差を外部トルクとして算出するステップ；ヤコビ行列算出部１０５が、関節軸の角度に基づいてロボットの作業座標系と関節座標系との間のヤコビ行列を算出するステップ；外力算出部１０９が、ヤコビ行列及び外部トルクから、先端位置に作用する外力を算出するステップ；修正量算出部１１０が、コンプライアンスモデル記憶部２０５に記憶されている先端位置におけるコンプライアンスモデルを用いて、外力に応じて位置指令値に対する修正量を算出するステップ等は、図６〜図８と等価なアルゴリズムのプログラムにより、図１に示したロボット制御装置を制御して実行できる。このプログラムは、例えば位置データ記憶部２００等の記憶装置に記憶させればよい。又、このプログラムは、コンピュータ読取り可能な記録媒体に保存し、この記録媒体を位置データ記憶部２００等に読み込ませることにより、本発明の実施の形態の一連の手順を実行することができる。更に、インターネット等の情報処理ネットワークを介して、位置データ記憶部２００等に格納することが可能である。

（第１の変形例）
本発明の実施の形態の第１の変形例として、コンプライアンスモデルのパラメータ（ゲイン）の変更方法を説明する。ロボットの関節ごとに減速機のスペックの違いなどから摩擦力トルクの大きさは異なっているため、コンプライアンスモデルを持たない場合、先端における外力に対する硬さ（微小変位の大きさ）は作業座標系の軸方向において等方的ではない。そこで関節軸ごとに設定している不感帯の閾値、すなわち最小外部トルクτの違いをパラメータに反映させることにより、より外力の推定精度を向上させるものである。外部トルクと作業座標系における外力の間には式（４）の関係があることから、修正量算出部１１０は、閾値記憶部２０３に記憶している最小外部トルクτ_dmin及びヤコビ行列記憶部２０４に記憶しているヤコビ行列の転置行列から、式（９）のように異方性パラメータベクトルｋを算出する。

修正量算出部１１０は、異方性パラメータベクトルｋを用いて式（１０）のようにコンプライアンスモデルのパラメータを修正する。

ここで、Ｋ_fiは作業座標系における第ｉ軸（ｘ、ｙ、ｚ、ａ、ｂ、ｃ）における力フィードバック行列の対角成分要素、Ｋ_fi’は修正したＫ_fi、｜ｋ｜はｋのノルムである。これによりロボットの作業形態、すなわち先端方向と作業時の動作方向に応じて異方的に設定されているコンプライアンスモデルのパラメータについても矛盾なく表現できる。

（第２の変形例）
本発明の実施の形態の第２の変形例として、式（８）のようにヤコビ行列を用いずに、先端位置から関節角を直接算出できる逆変換を用いる場合を説明する。この場合、関節角差分算出部１１３は、位置指令値生成部１１１により生成した先端位置指令値ｘ_Rに、修正量算出部１１０により算出した先端位置の修正量Δｘ_compを加算した新たな先端位置ｘを式（１１）のように算出する。

トルク指令値算出部１１４は、新たな先端位置ｘに逆変換Λ^-1を乗じてトルク指令値θ_Rを式（１２）のように算出する。

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

例えば、本発明の実施の形態において、慣性力トルクの算出の際に、速度算出ユニット４１０及び加速度算出ユニット４１１による関節軸角度の時間二回差分で算出された関節角加速度を用いているが、制御周期によっては時間遅れが大きくなりトルク誤差が大きくなりやすくなる。その場合は、図９に示すようにリンク３０１に搭載した加速度センサ３１１からリンク３０１の加速度を検出し、検出した加速度を加速度算出ユニット４１１が関節角加速度へ変換して、この関節角加速度を基に慣性力トルクを算出してもよい。

又、駆動トルク推定処理としては、摩擦力トルク、慣性力トルク及び重力トルクから駆動トルクを推定する一例を示したが、これに限定されるものではない。例えば、遠心力やコリオリの力等のパラメータを更に考慮しても良い。

又、ＣＰＵ１００、位置データ記憶部２００、摩擦係数記憶部２０１、リンクパラメータ記憶部２０２、閾値記憶部２０３、ヤコビ行列記憶部２０４及びコンプライアンスモデル記憶部２０５等が制御対象であるロボットの内部に埋め込まれて一体化されていても良い。又、ＣＰＵ１００、位置データ記憶部２００、摩擦係数記憶部２０１、リンクパラメータ記憶部２０２、閾値記憶部２０３、ヤコビ行列記憶部２０４及びコンプライアンスモデル記憶部２０５等が制御対象であるロボットの外部にあり、ロボットを有線或いは無線で遠隔制御することも可能である。

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

本発明の実施の形態に係るロボット制御装置の一例を示すブロック図である。 本発明の実施の形態に係る制御対象の一例を示す概略図である。 本発明の実施の形態に係る制御対象の一例を示す他の概略図である。 本発明の実施の形態に係る駆動トルク推定部の一例を示すブロック図である。 本発明の実施の形態に係る外力算出部の一例を示すブロック図である。 本発明の実施の形態に係るロボット制御方法の一例を示すフローチャートである。 本発明の実施の形態に係る駆動トルク推定処理の一例を示すフローチャートである。 本発明の実施の形態に係る外力算出処理の一例を示すフローチャートである。 本発明のその他の実施の形態に係るロボット制御装置の一例を示すブロック図である。

符号の説明

１００…中央演算処理装置（ＣＰＵ）
１０１…駆動部
１０２…駆動軸角度検出部
１０３…関節軸角度算出部
１０４…先端位置算出部
１０５…ヤコビ行列算出部
１０７…駆動トルク推定部
１０８…外部トルク算出部
１０９…外力算出部
１１０…修正量算出部
１１１…位置指令値生成部
１１２…誤差算出部
１１３…関節角差分算出部
１１４…トルク指令値算出部
１１５…アクチュエータ
２００…位置データ記憶部
２０１…摩擦係数記憶部
２０２…リンクパラメータ記憶部
２０３…閾値記憶部
２０４…ヤコビ行列記憶部
２０５…コンプライアンスモデル記憶部
３００…本体
３０１，３０２…リンク
３０３…駆動プーリ
３０４…従動プーリ
３０５…関節軸角度検出部
３０６…駆動軸（関節）
３０７…関節軸（関節）
３０８…伝達機構
３０９…減速機
３１０…可動部
３１１…加速度センサ
４００…駆動軸速度算出部
４０１…摩擦力トルク算出部
４０２…慣性力トルク算出部
４０３…重力トルク算出部
４０４…加算部
４１０…速度算出ユニット
４１１…加速度算出ユニット
４１２…慣性モーメント算出ユニット
４１３…慣性力トルク算出ユニット
５０１…不感帯処理ユニット
５０２…飽和処理ユニット
５０３…力変換ユニット
５０４…座標変換ユニット
５０５…閾値設定ユニット

Claims (6)

1. 関節軸及び前記関節軸に駆動力を伝達する駆動軸を有するロボットを制御するロボット制御装置であって、
   前記関節軸を駆動するアクチュエータと、
   前記駆動軸の角度を検出する駆動軸角度検出部と、
   前記駆動軸の角度から前記関節軸の角度を算出する関節軸角度算出部と、
   前記関節軸の角度から前記ロボットの先端位置を算出する先端位置算出部と、
   前記先端位置の位置指令値を生成する位置指令値生成部と、
   前記先端位置と前記位置指令値との誤差を算出する誤差算出部と、
   前記誤差から逆運動学計算により関節角差分を算出する関節角差分算出部と、
   前記関節角差分を積分することによりトルク指令値を算出するトルク指令値算出部と、
   前記トルク指令値に基づいて前記アクチュエータを駆動させる駆動部と、
   前記関節軸の角度から前記アクチュエータを駆動させるための駆動トルクを推定する駆動トルク推定部と、
   前記推定された駆動トルク及び前記トルク指令値との差を外部トルクとして算出する外部トルク算出部と、
   前記関節軸の角度に基づいて前記ロボットの作業座標系と関節座標系との間のヤコビ行列を算出するヤコビ行列算出部と、
   前記ヤコビ行列及び前記外部トルクから、前記先端位置に作用する外力を算出する外力算出部と、
   前記先端位置におけるコンプライアンスモデルを記憶するコンプライアンスモデル記憶部と、
   前記コンプライアンスモデルを用いて、前記外力に応じて前記位置指令値に対する修正量を算出する修正量算出部と
   を備えることを特徴とするロボット制御装置。
2. 前記先端位置における摩擦係数を記憶する摩擦係数記憶部を更に備え、
   前記駆動トルク推定部が、
   前記関節軸の角度から前記駆動軸の速度を算出する駆動軸速度算出部と、
   前記駆動軸の速度及び前記摩擦係数から摩擦トルクを算出する摩擦力トルク算出部と、
   前記関節軸の角度から重力トルクを算出する重力トルク算出部と、
   前記関節軸の角度から慣性力トルクを算出する慣性力トルク算出部と、
   前記摩擦力トルク、前記重力トルク及び前記慣性力トルクを加算して前記駆動トルクとする加算部
   とを備えることを特徴とする請求項１に記載のロボット制御装置。
3. 前記慣性力トルク算出部は、
   前記関節軸の角度から関節角速度を算出する速度算出ユニットと、
   前記関節角速度から関節角加速度を算出する加速度算出ユニットと、
   前記関節軸の角度から前記ロボットのリンクの慣性モーメントを算出する慣性モーメント算出ユニットと、
   前記関節角加速度と前記慣性モーメントから慣性力トルクを算出する慣性力トルク算出ユニット
   とを備えることを特徴とする請求項２に記載のロボット制御装置。
4. 前記外部トルクに対する最小閾値及び最大閾値を記憶する閾値記憶部と、
   前記ロボットのリンクパラメータを記憶するリンクパラメータ記憶部とを更に備え、
   前記外力算出部が、
   前記外部トルクが前記最小閾値よりも小さい場合に前記外部トルクを０とみなす不感帯処理ユニットと、
   前記外部トルクが前記最大閾値よりも大きい場合に前記外部トルクを前記最大閾値の値とする飽和処理ユニットと、
   前記ヤコビ行列を用いて前記外部トルクを外力に変換するトルク−力変換ユニットと、
   前記リンクパラメータを用いて前記外力を実際の作用点での外力に変換する座標変換ユニットと、
   前記ヤコビ行列から前記最小閾値及び最大閾値をそれぞれ設定する閾値設定ユニット
   とを備えることを特徴とする請求項１〜３のいずれか１項に記載のロボット制御装置。
5. 前記修正量算出部が、前記ヤコビ行列から前記コンプライアンスモデルに対するゲインを変更することを特徴とする請求項１〜４のいずれか１項に記載のロボット制御装置。
6. 前記ロボットのリンクに搭載され、前記リンクの加速度を検出する加速度センサを更に備え、
   前記加速度算出ユニットが、前記検出された加速度から関節角加速度を算出することを特徴とする請求項３に記載のロボット制御装置。

Images